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1 . 1 Purpose  of  Function 

a)  Statement  of  Problem 

This  volume  contains  a detailed  description  of  the  simulator 
program  design,  developed  in  this  task  study. 

The  purpose  of  this  simulator  program  is  to  allow  the  determi- 
nation of  the  Inertial  Navigation  System  (INS)  navigation  errors 
in  position,  velocity  and  attitude  resulting  from  mechanization 
of  the  navigation  equations  on  a digital  computer  for  a "perfect" 
Inertial  Measurement  Unit  (IMU) . A perfect  IMU  consists  of  error 
free  instruments,  that  is  the  uncompensated  gyros  are  not  drifting 
and  sense  the  true  inertial  angular  velocity,  the  uncompensated 
accelerometers  sense  the  true  specific  force  and  the  gimbal  angle 
resolvers  generate  the  true  angles. 

The  program  provides  simulation  of  the  navigation  computation, 
for  three  different  IMU  mechanizations,  namely  a local  level 
wander  azimuth  platform  stabilization,  a space  stabilized  pie tform 
and  a strapdown  configuration.  All  three  mechanizations  use  a 
perfect  barometric  altimeter.  No  other  navigation  aids  are  employed 
in  the  computations. 

The  simulated  navigation  computations  are  performed  in  a 
local-level,  wander  azimuth  computational  frame. 

The  computations  are  assumed  to  be  performed  on  an  airborne 
digital  computer  using  a "floating  point"  representation  of  a 
specific  length  common  to  all  variables. 

b)  Simulated  Errors 

The  effect  of  the  following  error  sources  can  be  investi- 
gated. 

i)  Quantizations  of 

A)  Accelerometer  inputn 

B)  Gimbal  angle  resolver  or  incremental  angle  inputs 

C)  Torquing  commands  to  local  level  IMP 


ii)  Vertical  channel  damping  mechanization  - second  or 
third  order,  value  of  parameters 

iii)  Iteration  rates,  shorter  versus  longer  computational 
cycles 

iv)  Word  Length,  that  is  the  precision  to  which  the 
airborne  computer  is  performing  its  calculations 

v)  Navigation  Algorithm  implementation: 

A)  "Order"  of  navigation  equations 

B)  Time  between  orthormalizations  of  direction 
cosine  matrix  ('DCM') 

C)  Formulas  used  in  calculations  of  angular  velocity 

D)  Order  of  Direction  Cosine  Matrix  (DCM)  updates 

vi)  Library  routine  implementation  - manner  in  which 

sine/cosine,  arctangent  and  square  root  routines 
are  implemented 

vii)  IMU  type  used  in  the  simulation 

A)  Local  level,  wander  azimuth  platform 

B)  Space  stable  platform 

C)  Strapdown  - in  this  configuration  one  may  also 
investigate 

I)  Effect  of  quaternion  vs.  DCM  implementation 
II)  Effect  of  different  update  orders,  or  ortho- 
normalization rates. 

The  effect  of  different  gravity  models  on  the  navigation  errors 
is  not  included  in  this  simulation. 

One  or  more  of  these  error  sources  can  be  introduced  simul- 
taneously, in  order  to  investigate  the  effects  of  combinations  of 


errors . 


Description  of  Inputs,  Outputs  and  Usage 


To  use  the  simulator  to  investigate  the  effects  of  given 
computation  errors  on  a given  mission's  navigation  errors  one  must: 

i)  Specify  the  "flight  profile”  for  AFAL's  supplied 
flight  profile  generator  computer  program  PROFGEN . 
ii)  Run  the  profile  through  PROFGEN,  producing  an  output 
file  (a  magnetic  tape  presumably)  containing  the 
specific  forces  and  gimbal  angles  that  would  be 


sensed  during  the  mission f along  with  the  position 
the  velocity  and  the  attitude, 

iii)  Establish  the  baseline  for  comparison  either  by: 

A)  using  the  PROFGEN  output,  or  by 

B)  running  the  simulator  with  no  intentional 
computation  errors,  that  is,  full  precision,  no 
quantization,  etc. 

iv)  Specify  the  configuration  to  the  simulator  and  run 
the  simulator  using  the  file  from  step  ii)  as  input. 
This  will  create  an  output  file  (a  magnetic  tape 
presumably)  with  the  computed  position,  velocity, 
and  attitude. 

v)  If  one  used  iii  B) , difference  the  output  of  iv) 
against  the  baseline. 

vi)  Perform  post  processing  of  the  differences  (compu- 
tation of  the  RMS  navigation  errors  induced  by 
various  computation  errors, etc.) 

The  inputs,  outputs  and  method  of  operation  are  described  in 
more  detail  in  Section  III  of  this  volume. 

In  brief,  the  inputs  consist  of  specifying  numerical  values 
for  the  errors  (e.g.  the  number  of  bits  of  precision)  and  numerical 
values  for  various  control  parameters  (e.g.  how  often  to  print 
results) . The  outputs  are  both  an  output  file  and  printed  output. 
The  output  file  contains  the  computed  position,  the  velocity  and 
the  altitude,  and  the  differences  between  these  computed  values, 
and  the  position,  the  velocity  and  the  attitude  generated  by 
PROFGEN.  The  printed  output  also  has  these  values  and  can  have, 
in  addition,  various  "debugging"  diagnostic  printouts. 

1.2  Overview:  Functional  and  Structural 
a)  Simulator  Design 

A functional  block  diagram  of  the  simulator  program 
is  shown  in  Figure  1.  The  simulator  consists  of  the  following 
parts: 

1.  A flight  profile  generator  (PROFGEN) 

2.  An  IMU  simulator 

3.  A navigation  computer  simulator 

4.  An  error  processor. 


Figure  1.  Numerical  Simulator  Functional  Block  Diagram 


The  PROFGEN  generates  the  specific  forces,  the  aircraft 
attitude,  the  position  and  the  velocity  for  a given  flight 
trajectory.  The  specific  forces  and  the  attitude  data  are 
transformed  by  the  IMU  simulator  from  the  PROFGEN  computational 
frame  (local  level,  geodetic,  wander  azimuth)  to  the  IMU 
instruments  frame.  The  data  is  then  transformed  in  the  naviga- 
tion computer  simulator  to  the  navigation  computer  computational 
frame  (local  level,  geodetic,  wander  azimuth)  and  the  aircraft 
position,  velocity  and  attitude  are  computed  on  the  simulated 
aircraft  navigation  computer.  The  computed  navigation  data  is 
compared  with  the  PROFGEN  generated  navigation  data  in  th'*  error 
processor.  The  error  processor  outputs  are  the  instanteous  navi 
gation  errors  in  the  computed  position,  velocity  and  attitude 
caused  by  mechanization  of  the  navigation  equations  on  an 
airborne  digital  computer  for  a specified  flight  mission. 

b)  Calculations  Performed 

Several  preliminary  concepts  must  be  set  forth  before 
examining  the  calculations  performed.  The  first  is  that  of  a 
•cycle'  as  it  appears  in  the  terras  'attitude  cycle',  'navigation 
cycle',  'filter  cycle'  and  'sample  cycle*.  The  sample  cycle 
refers  to  the  time  between  inputs  from  the  PROFGEN  output  file; 
as  an  example  if  there  are  128  outputs  per  second,  then  the 
sample  cycle  is  1/128  sec.  The  attitude  cycle  is  the  time  between 
attitude  calculations  in  the  simulated  airborne  computer;  as  an 
example,  if  the  airborne  computer  computes  attitude  64  times  per 
second,  then  the  attitude  cycle  is  1/64  sec.  Similarly,  there 
are  cycle  times  defined  for  the  attitude  filter  (which  performs 
a statistical  filtering  of  the  attitude  data)  and  for  the  naviga- 
tion calculations  (which  computes  position  and  velocity) . 

The  simulator  is  set  up  so  that  these  cycles  must  bear  cer- 
tain relationships  to  one  another. * To  be  specific,  the  attitude 
cycle  must  be  an  integral  multiple  of  sample  cycles,  the  filter 
cycle  must  be  an  integral  number  of  attitude  cycles,  and  the 
navigation  cycle  must  be  an  integral  number  of  filter  cycles. 

The  second  concept  is  that  of  "IMU  simulator"  coding  versus 
"flight  code"  coding.  The  "flight  code"  consists  of  that  software 
which  would  actually  be  programmed  in  to  the  airborne  computer. 
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In  brief /this  coding  takes  the  accelerometer  data  (the  delta= 
velocities)  and  attitude  information,  processes  it,  and  produces 
the  navigation  data  (the  velocity,  the  position  and  the  attitude) . 

The  "IMU  simulator",  on  the  other  hand,  accepts  information 
from  PROFGEN  and  performs  those  calculations  necessary  to  generate 
the  inputs  to  the  flight  code.  The  IMU  simulator  takes  into 
account 

i)  The  PROFGEN  sample  cycle  being  different  from  either 
the  attitude,  filter,  or  navigation  cycles, 
ii)  Misalignments  between  the  actual  platform  and  the 
flight  codes  knowledge  of  platform  alignment, 
iii)  An  IMU  orientation  other  than  the  local  level;  local 
level  is  used  by  PROFGEN. 

The  third,  and  final,  preliminary  concept  is  that  of  IMU 
stabilization,  which  describes  the  orientation  that  the  acceler- 
ometers and  attitude  sensors  will  have  during  the  mission.  Three 
options  are  available: 

i)  Local  Level  Wander  Azimuth  - in  this  configuration 
the  platform  is  'torqued'  (rotated  with  respect  to 
inertial  space)  in  such  a way  as  to  keep  two  of  the 
axes  level  (level  is  defined  as  being  perpendicular 
to  the  geodetic  gravity  vector) , and  to  compensate  for 
the  earth  rate.  The  effect  of  this  is  that  if  the  IMU 
were  stationary  on  some  point  on  the  earth,  an  observer 
at  that  point  would  not  see  a rotation  about  any  of 
the  axes.  Any  easterly  motion  of  the  IMU  at  non-zero 
latitude,  would  cause  a rotation  in  azimuth  (that  is, 
about  the  up  axis)  to  be  seen  by  an  observer  riding 
along  with  the  IMU. 

ii)  Space  Stable  - in  this  configuration  the  platform  main- 
tains a constant  orientation  with  respect  to  inertial 
space. 

iii)  Strapdown  - in  this  configuration  the  accelerometers  are 
rigidly  attached  to  the  body  of  the  vehicle  so  that  the 
accelerometers  rotate  (with  respect  to  inertial  space) 
the  same  way  that  the  body  does. 
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For  the  local  level  case,  the  IMU  simulator  forms  up  the 
attitude  angles  once  every  DELTA.  The  specific  forces  are 
accumulated  over  DELT  Cthe  navigation  cycle) . During  the  atti- 
tude cycle  in  which  the  navigation  calculations  are  to  be  per- 
formed, the  navigation  calculations  are  performed  prior  to  the 
attitude  calculations.  The  attitude  filter  is  run  every  DELTF 
(the  filter  cycle)  after  the  attitude  computations  for  that 
attitude  cycle. 

The  space  stable  case  is  nearly  identical,  the  sole  differ- 
ence being  that,  just  prior  to  the  navigation  calculations,  cal- 
culations are  performed  to  rotate  the  delta-velocity  (which  is 
coming  from  a simulated  space  stable  IMU)  into  the  local  level 
wander  azimuth  computational  frame. 
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The  calculations  performed  for  the  strapdown  IMU  are  similar, 
and  are  shown  in  Figure  3. 


IMU 

SIMULATOR 


FLIGHT 

CODE 
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Figure  3. 


Strapdown  Calculations 
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The  strapdown  calculations  are  also  similar  to  the  local 
level.  The  difference  is  the  existence  of  the  "high-speed" 
delta-v  summation,  which  is  run  every  attitude  cycle.  The 
purpose  of  this  summation  is  to  accumulate  the  delta-v  in  the 
computation  frame.  This  high  speed  summation  is  necessitated  by 
the  high  angular  rates  {of  body  with  respect  to  computational 
frame)  achieved. 

c)  "Flight  Code" 

Flight  code,  as  discussed  before,  consists  of  those  cal- 
culations that  would  be  performed  in  an  airborne  computer.  For 
the  purposes  of  this  simulator,  it  is  assumed  that  all  quantities 
are  stored  in  a floating  point  format.  It  is  further  assumed  that 
the  amount  of  storage  for  each  quantity  is  the  same,  that  is,  each 
quantity  is  computed  to  the  same  precision. 

Since  the  flight  code  computations  will  be  performed  in  real- 
time (that  is,  synchronized  with  events  in  the  real  (physical) 
world) , the  goal  of  the  design  of  the  computational  algorithms  is 
to  achieve  the  maximum  computational  precision  (minimize  the  maxi- 
mum error  due  to* the  approximations  selected)  while  keeping  the 
computational  load  (that  is,  maximum  computation  time  per  cycle) 
below  some  fixed  constraint. 

d)  Initialization 

The  initialization  performed  can  be  viewed  in  two  parts.  First 
is  the  setting  of  control  variables  regulating  cycles,  printout 
selection,  etc.  Second  is  the  alignment  of  the  perfect  IMU  and 
initialization  of  the  navigation  algorithms  that  would  take  place 
in  the  airborne  computer  during  alignment. 

This  initialization  is  performed  by  the  'executive'  (the  main 
program) , the  BLOCK  DATA  subprogram,  and  the  various  intialization 
subroutines . 

e)  Modularity  and  Interroutine  Communication 

The  simulator  is  constructed  of  a main  program  and  approximately 
51  subroutines.  The  subroutines  can  be  divided  into  four  categories! 
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1)  Initialization  subroutines 

2)  IMU  simulator  subroutines  (including  all  input 
and  output  routines) . 

3)  Plight  code  subroutines. 

4)  Library  subroutines  (includes  trigonometric  func- 
tions, matrix  multiply,  etc.). 

The  main  program  (also  called  the  ’executive'  or  the  'exec') 
calls,  in  the  proper  sequence,  the  initialization  subroutines, 
the  IMU  simulator  subroutines  and  the  flight  code  subroutines. 

The  library  subroutines  are  made  use  of  by  the  other  three 
categories  of  subroutines.  The  interrelationship  of  subroutine 
is  displayed  graphically  in  the  appendix  "Subroutine  Cj.11  Structure". 

The  individual  subroutines  were  designed  to  be  as  modular 
(that  is,  performing  a well  defined  role  independently  of  the 
actions  of  other  subroutines)  as  possible.  The  interroutine 
communication  is  handled  as  follows: 

1)  All  variables  are  kept  in  labelled  COMMON  blocks. 

2)  Certain  of  the  lower- level  subroutines  are  'passed' 
variables  through  an  argument  list. 

1.3  Operation 

a)  Method  of  Operation 

The  simulator  is  setup  to  run  on  the  CYBER  74,  under 
the  control  of  either  SCOPE  or  INTERCOM.  The  user  of  the  simu- 
lator (called  the  "operator"  throughout)  must  have  the  PROFGEN 
binary  output  (also  called  "output  tape")  available  to  the 
program.  In  addition,  the  operator  must  prepare  input  cards 
as  described  in  the  " Program  Operation"  section  of  this  docu- 
mentation. The  "Local  Files"  necessary  to  run  the  program  are 
described  in  an  appendix. 


One  needs  to  examine  both  1}  the  magnitude  of  the  errors 
at  the  end  of  the  simulated  mission  and  ii)  the  RMS  (with  respect 
to  time)  errors  in  order  to  be  able  to  evaluate  the  performance 
of  a given  (simulated)  system  during  the  mission.  All  such  post 
processing  must  be  programmed  by  the  operator. 


HP 


2.0  DETAILED  PROGRAM  DESCRIPTION 

2.1  Introduction 

This  section  contains  all  the  detail  information  related  to 
the  main  program  and  all  the  subroutines;  for  each  routine  there  is: 

a)  A discussion  of  the  function  and  equations  of  the  routine. 

b)  A list  of  the  inputs,  outputs  and  internal  variables. 

c)  A functional  flowchart,  where  applicable. 

Note  that 

a)  Calls  to  the  DOUT  (debugging  output)  routine  are  not 
shown  or  discussed. 

b)  In  the  flight  code  section 

i)  Calls  to  the  truncation  routine,  VP,  are  not  shown 
ii)  Calls  to  VSQRT,  VMM  etc.  are  shown  as  calls  to 
SQRT,  MM,  etc. 

c)  The  arrays  A & B,  when  listed  as  internal  variables, 
actually  use  the  A & B of  the  NAVCOM  COMMON  block  as 
a work  area. 

Figure  4 of  this  section  contains  the  symbol  list  for  all 
variables  in  COMMON,  excluding  only  those  variables  (whose 
nature  is  fairly  trivial)  found  in  the  OUTPUT,  UNVRSL  and  PGRAV 
common  block. 

With  regard  to  the  symbol  listing,  note  the  following: 

1)  Except  as  noted,  all  variables  are  used  for  all  three 
(local  level  'LL',  space  stable  'SS',  and  strapdown  *SD' 

I MU  types. 

2)  Frames  (except  as  listed)  used  in  this  program  are  defined 
in  Volume  II,  Appendix  A of  this  report. 

a)  21  is  the  frame  of  the  accelerometer  axes 

b)  5Q  is  PROFGENs  body  frame 

c)  2QP  is  PROFGENs  computational  frame 

3)  Symbol  XXINTG  designates  LLINTG,  SSINTG  or  SDINTG,  which 
ever  is  appropriate.  Similar  designation  is  used  for 
symbols  XXATUD,  XXINIT.  Symbol  XXATT  referes  to  LLATT, 

SSATT  and  a portion  of  HSINTG. 
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' 

j 

4 


U 


4}  For  the  purposes  of  this  section,  ANGVL2  and  TORCOR  are 
used  as  part  of  INRNAV. 

5)  1 (computed) ' refers  to  those  quantities  computed  by 

'flight  code'  as  opposed  to  those  generated  by  'IMO 
simulator' . 

2.2  Some  Conventions 

1)  Two  dimensional  arrays,  e.g.:  A (row,  column)  are 
interpreted  as 


A(l,l) 

A (1, 2) 

A(l,3) 

A(2,l) 

A (2,2) 

A(2,3) 

A(3,i) 

A(3,2) 

A(3,3) 

2)  Relationship  among  Rates 

A)  DELTA  * ITRNAV  = DELT 

ITRNAV  = n* ITRATT  for  some  integer  n 
DELTS  * ITRATT  = DELTA 
ITRFlL  = j * ITRNAV  for  some  integer  j 
ITRATT  = k * ITRFIL  for  some  integer  k 

B)  DELTS  is  found  by  examining  trajectory  input  tape 
ITRATT  is  an  input,  then  DELTA  is  computed,  ITRATT 
recomputed 

ITRNAV  is  an  input,  then  DELT  is  computed,  ITRNAV 

recomputed 

INCOR  is  an  input 

3)  Cn$m  (3,3)  is  transformation  matrix  from  m to  n frame 
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4)  Q2P$5(4)  is  the  quaternion  representation  of  the  rotation 

involved  in  the  transformation  from  the  5 frame  to  the 
2P  frame. 


Q2P$5 (1)  - scalar  element 
Q2P$5 (2)  - x component 
Q2P$5 (3)  - y component 
Q2P$5 (4)  - 2 component 

vector  (3)  is  a vector,  stored  as 
vector  (1)  -H  component 
vector  (2)  - £ component 
vector  (3)  - 2 component 


Figure  4.  Program  Symbol  Listing 
Variables  - Input's  Com 


Vertical  Damping  Coefficient 


Symbol  Listing  - 
s - Inputs  Common  Block  (Cont- 
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Trajectory  Input  Common  Block 
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Program  Symbol  Listing 


C0P$2P  (1,1)  thru  (3,3) 
-unused- 

THTERT  (1)  thru  (3) 
THTTRQ  (1)  thru  (3) 

WXV  (1)  thru  (3) 


VDMP 

G$2P  (1)  thru  (3) 

elements  (1,2),  (1,3)  and  (2,3)  of  DCM 
representing  incremental  rotation  of  C$RHO 
C10$2P  (1,1)  thru  (3,3) 

C2P$5  (1,1)  thru  (3,3) 


C0$5  (1,1)  thru  (3,3) 

TIME 

element  (3,2),  minus  (3,1),  (2,1)  of  Ci 
(C2P$5)T (C2P$5T)  {matrix  multiply  implied} 
Q2P$5  (1)  thru  (4) 





2.3  DESCRIPTION  OF  ROUTINES 


1 

2.3  DESCRIPTION  OF  ROUTINES  'fl 

B 

1 

MAIN  ] 1 

1 ■ 

2.3.1  Function  & Equations  s 

B.- 

B 

This  is  the  executive  routine  for  the  simulator.  It  receives  l| 

If 

control  when  the  simulator  is  loaded  for  execution  and  then  calls  the  1 

I 

appropriate  subroutines  in  the  appropriate  sequence.  This  routine  h 

also  determines  when  it  is  time  for  periodic  printout  and  plot  |j 

B 

tape  output  and  calls  the  necessary  routines.  It  is  also  responsible  j 

I 

for  reading  the  operator  specified , and  PROFGEN  generated, initiali-  | 

zation  parameters.  Finally  this  routine  terminates  execution  when 

1 

the  operator  specified  stop  time  is  exceeded  or  errors  which  ex-  jj 

I - 

j|- 

1 

1 

1 

ceed  the  operator  specified  error  limit  are  detected.  * 

i I 

Input  Variables 

51 

ITITLE  - 80  character  operator  specified  title  placed  on  each 

f. 

printout  page.  1 

4 

IP AGE  - Number  of  next  page  to  be  printed.  | 

-J 

IMUTYP  - Operator  specified  code  to  select  the  type  of  IMU 

* 1 

to  be  simulated.  1 

•1 

1 = Local  Level  Wander  Azimuth  (LLWA) . 3 

J 

2 = Space  Stable  (SS) . ... 

3 = Strapdown  (SD) . | 

ITRNAV  - Number  of  attitude  update  iterations  per  navigation 

code . 1 

ITRATT  - Number  of  trajectory  samples  per  attitude  update 

cycle.  jj 

ISTOP  - Error  severity  indicator.  J1 

TIME  - Elapsed  time  for  trajectory.  J 

STOP  - Operator  specified  stop  time.  JS 

TPREV  - Time  for  previous  cycle.  j|| 

SF$T  - Specific  force  from  trajectory.  ^ 

ITRFIL  - Number  of  attitude  update  iterations  per  attitude  if 

-5S 

filter  cycle.  Jj 

PTIME  - Elapsed  time  for  next  printout.  jj 

PLTIME  - Elapsed  time  for  next  plot  tape  record  output. 

m 

’ll 

33  ; 1 

- 

| | 

If 

^ (J 

33 


PLOTIM  - 
IOPNLP  - 


SSYO  j 
SSZO  ) 
HSOTlM  - 
HSOINC  - 
GAINS 


Periodic  printout  period. 

Plot  tape  output  period. 

Flag  to  control  local  level  wander  azimuth  I MU  model. 

"0"  value  indicates  misalignment  due  to  torguing 
commands  is  to  be  modeled,  "l"  value  indicates  no 
misalignment  modeled. 

X,  Y,  and  2 Euler  angles  for  initial  space  stable 
platform  alignment. 

Simulated  time  at  which  Q2P$5  or  C2P$5  is  to  be  normalized. 
Time  between  normalizations  for  Q2P*5  or  C2P$5. 

Gains  for  attitude  filter. 


Internal  Variables 


None 


Output  Variables 


TPREV  - See  above. 

SF$TP  - Specific  force  from  previous  cycle. 

IPAGE  - see  above. 


ISTOP=10 


TIME  > STOP, 


LUNTG 
LLWA  IMU 


k^TPREV.OR^SUL 

VTX 


SF$TP=SF$T 


LtATUD 

COMPUTE 

GIMBAL 

ANGLES 


r LLATT  > 

CONSTRUCT 

C2PS5 


l=ITRNAV 


r INRNAV  ^ 

NAVIGATION 
EQUATIONS  , 


ATUDE 

COMPUTE 

ATTITUDE 

ANGLES 


r MOD  > 
(I.ITRFIL) 
v =0  > 
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INPUT  > 

TRAJECTORY  y 

Vdata  / 


.TIME  >STOP^ 


/ SDINTG  \ 
/ COMPUTE  \ 

(acceleration) 

\ IN  BODY  / 
\ FRAME  / 


/ SOATUD  \ 
f COMPUTE  \ 
k INCREMENTAL > 
\ ANGLES  / 


SUM  DELTA 
VELOCITY. 

, C2PS5  / 


MOVE  DELTA 
> VELOCITY  . 
\ DATA  / 


yr  TIME  X. 
^TPREV.  ORS 
VlSTOP 

\o/ 

|YES 


TPREV-TIME 


NAVIGATION 
k EQUATIONS  i 


SUM 

OELTA 

VELOCITY 


COMPUTE 

ATTITUDE 

ANGLES 


BLOCK  DATA  SUBPROGRAM 


{ 


! 

! 

| 

i 

i 

i 


i 


2.3.2  Function  & Equations 

The  sole  purpose  of  this  routine  is  to  perform  initialisation 
of  the  variables  in  the  various  labelled  common  blocks.  As  such, 

it  does  not  consist  of  any  executable  code,  but  merely  of  DATA 
statements. 


Inputs,  Outputs  of  Internal  Variables 
Refer  to  listing. 


m 


INREC 


2.3>  3 'Function  & Equations 

The  purpose  of  INREC  is  to  read  PROFGEN  data  from  an  input 
t..pe.  Input  PROFGEN  records  are  expected  to  be  fixed  length  (14 
words  per  record)  and  contain  the  following  variables  in  single 
precision  floating  point: 

TIME  - Elapsed  time  in  seconds. 

LAT$T  - Geodetic  latitude  (RAD) . 

LONG$T  - Geodetic  longitude  (RAD) . 

ALF$T  - Wander  angle  (RAD) . 

HB  - Altitude  (FT). 

ETA$T  - Roll,  pitch,  and  yaw  atitude  angles  (RAD). 

V$T  - Instantaneous  velocity  in  local  level  wander  azimuth 
frame  (north,  west,  up  axes)  (FT/SEC) . 

SF$T  - Specific  force  in  local  level  wander  azimuth  frame 
(FT/SEC2) . 

The  routine  also  provides  the  sine  and  cosine  of  both  LAT$T  and  ALF$T. 

The  first  pass  through  the  routine  is  used  for  initialization 
during  which  the  following  steps  are  taken: 

1.  Find  the  operator  specified  start  time  on  the  trajectory  tape. 

If  unable  to  find  start  time,  inform  operator  with  a printed 
message. 

2.  Ensure  that  the  attitude  update  period,  the  navigation  equation 
update  period,  and  the  attitude  filter  update  period  are 
multiples  of  the  trajectory  tape  period.  Also  ensure  that  the 
navigation  period  and  the  attitude  filter  period  are  multiples 
of  the  attitude  period. 

3.  Printout  the  timing  relations  between  trajectory,  navigation,  and 
attitude  periods.  Also  printout  messages  to  indicate  any  error 
conditions. 

The  following  actions  are  taken  when  error  conditions  are  detected: 
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1.  Attitude  update  period  is  set  to  the  trajectory  sample  period 
if  the  operator  specified  period  is  not  an  even  multiple  of 
the  trajectory  period. 

2.  Navigation  equation  period  is  set  to  the  attitude  update  period 
if  the  operator  specified  period  is  not  an  even  multiple  of 
the  attitude  update  period. 

3.  Attitude  filter  period  is  set  to  the  attitude  update  period 
if  the  operator  specified  period  is  not  an  even  multiple  of 
the  attitude  update  period. 

Input  Variables 

INIT  - Initialization  fdag.  "0"  value  indicates  first  pass 
. through  routine . 

ITRATT  - Operator  specified  attitude  update.  Rate  in  cycles 
per  second.  Value  less  than  or  equal  to  zero  forces 
rate  to  that  of  trajectory  samples. 

ITRNAV  - Operator  specified  navigation  equation. 

Iteration  rate  in  cycles  per  second.  Value  less 
than  or  equal  to  zero  forces  rate  to  the  rate  of 
attitude  computations. 

ITRFIL  - Operator  specified  attitude  filter. 

Iteration  rate  in  cycles  per  second.  Value  less 
than  or  equal  to  zero  forces  rate  to  the  rate  of 
attitude ' computations . 

IPC(25)  -Attitude  filter  flag.  A non  zero  value  specified  by 

operator  indicates  attitude  filtering  is  to  be  performed. 

START  - Operator  specified  start  time  in  elapsed  seconds. 

Internal  Variables 

PRDAT  - Trajectory  data  input  buffer.  Equivalenced  to 
trajectory  variables. 

PRD  - Temporary  buffer  for  second  trajectory  data  record. 

Used  in  order  to  determine  trajectory  sample  period. 

TIM  - Elapsed  time  in  seconds  of  second  trajectory  data  record. 
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ITMP1 

ITMP2 

ITMP3 

ITMP4 

ITMP5 

TEMP 


Output  Variables 


TIME 

LAT$T 

LONG$T 

ALF$T 

ETA$T 

V$T 

SF$T 

SLAT$T 

CLAT$T 

SALF$T 

CALF$T 


Scratch  variables  used  to  calculate  timing 
multiples  and  for  printout. 


DELTS  - 
DELTA  - 
DELT 
ITRATT  - 


ITRNAV  - 


ITRFIL  - 


ISTOP  - 


Data  from  trajectory  tape  (see  above) 


Sine  of  LAT$T. 

Cosine  of  LAT$T. 

Sine  of  ALF$T. 

Cosine  of  ALF$T. 

Trajectory  sample  period. 

Attitude  computations  iteration  period. 
Navigation  equations  iteration  period. 

Number  of  trajectory  samples  per  attitude 
computation  iteration. 

Number  of  attitude  computation  iterations  per 
navigation  cycle. 

Number  of  attitude  computation  iterations''- per 
attitude  filter  cycle. 

Error  severity  level. 


EaSSsB 


TORCOR 


2.3.4  Function  & Equations 

A)  The  purpose  of  TORCOR  is  to  implement  the  portion 
of  the  navigation  equation  that  computes 

a)  The  angular  rate  of  the  earth  with  respect 
to  inertial  space  (or,  after  being  multi- 
plied by  DELT,  THTERT) . 

b)  The  angular  rate  of  the  platform  with  respect 
to  the  inertial  space  (p  + 0)  (or,  after 
being  multiplied  by  DELT,  THTTRQ) . 


1/ 


c)  The(w  x v)x  At  term  of  the  velocity  update 
' (THTTRQ  + THTERT)  X AV$2P) , where  x is 
vector  cross  product  operation  and  AV$2P  is 
the  velocity  passed  to  the  subroutine  l 


B)  THTERT  is  computed  as 

, T 


DELT  * (A0P$2P) 


0. 

0. 

|WERT! 

where  WERT  is  the  rate  of  the  earth  in  the  North  - East  - Up  frame 

The  axes  orientation  of  the  inertial  OP  frame  is: 

XQ  = Equatorial  - initial  meridian 

f 

Yq  = Equatorial  - initial  east 

Z = Polar  - north  polar 
o 

A0p$2P  represents  the  DCM  (from  2P  to  OP)  passed  to  TORCOR. 

Note  that  only  the  bottow  row  of  A0P$2P  is  used.  THTTRQ  is 
computed  as 

THTERT  + RHO  * DELT 


where  RHO  is  the  vector  (computed  externally)  giving  the  rate 
of  the  2P  frame  with  respect  tc  the  OP  frame. 


Inputs,  Outputs  and  Internal  Variables 


AV$2P 

parameter  representing  velocity  as  seen 
in  2P  (input) 

AOP$2P 

Paramater  represents  DCM  transforming  2P 
to  OP  (input) 

RHO 

- 

rate  of  2P  w.r.t.  OP  (input) 

WERT 

- 

«15°/hr  (rotational  rate  of  earth)  (input) 

DELT 

- 

time  between  navigation  cycles  (input) 

THTERT 

- 

described  above  (output) 

THTTRQ 

- 

described  above  (output) 

THTCOR 

- 

THTTRQ  + THTERT  (output) 

A 

- 

scratch 

WXV 

- 

described  above  (output) 

J 


» ::  i 

f V 

I I 
I * 

I S 


I 'I 
{ | 


i j 


i 

i 


§ 


I : 

5 


Is  . 
1 
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INRNAV 


2.3.5  Function  & Equations 

A)  The  purpose  of  INRNAV  is  to  perform  the  calculations 

of  position  (that  is  latitude,  longitude  and  altitude)  and  "ground- 

velocity  (that  is,  instantaneous  vehicle  velocity  expressed  in  the 

earth  fixed  frame).  The  ground  velocity,  as  written  in  the 

OP  frame  is  Rop  where  Rop  is  the  vector  from  the  center  of  the 

earth-fixed  frame  to  the  vehicle.  We  wish  to  find  this  vector 

in  the  2P  frame;  this  can  be  written  as  V2p  = C2p  Rop.  Since 

op  - 

the  physical  data  relevant  to  the  calculation  is  accelerometer 
data,  one  wishes  to  find 

t 


V2p(t)  = 


v2p(to)  + 


/i 

to 


2p 


dt 


One  relates  V2p  to  the  physical  data  as  follows: 


V2p 


d 

dt 


(c2p  ^op)  = C^P  (p°P 

op  - 


op  -2p,op 


x Rop} 


+ C2p  r’°P  = p?P  x V2p  + C2p  R*op 
op  — _2p,op  — op  — 


{where  p?p  is  the  angular  rate  of  the  2P  frame  wrt  2P  frame 

^pfop 

as  expressed  in  the  2P  framef 

Now,  let  R°  be  position  vector  in  0 (space  stable  equatorial 
Up  - East  - North)  frame. 


tnen 


R°  = A C°  R°p  = x C~  Rwr  + C“  R" 

— dt  op  — — o,op  op  — op  — 


where  op  is  rate  of  OP  frame  with  respect  to  0 frame,  as 
expressed  in  0 frame.  Therefore 


R*  = A («°  x R°)  +.J  (C°  lop) 
- dt  -o,op  — dt  op  R ' 
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B*'  x 

gp  I 

Efe  5 


= 0,°  X R°  + nn  X (C®  R°P) 
-o,op  ■-  —o,op  at  op  — 

+ 0°  „„  X C°  EOP  + c°  r'OP 
-o , op  op  — op  — 


{since  Q°  is  a constant { 
r op , o ’ 

= Q°  X C°  R P + nn  X rm  X RC 
i^op  op  - _o  ,op  _o,  op  - 


:°  R°P)  + Con  * R°P 


+ £o,op  x (cop  r > + C5P  r 


Multiplying  by  C 


C2p*  R°  = c2p’uop  - 2 a2p  X V2p 
— op  — op  tO  *“ 


+ c2P  (Q°  _ X a°x  R°) 

O -O/OP  o,op  — 


rearranging 


c2p’i°P  = c2p'i°  + 2B2P  X v2p  - c2p(f£  m X m X R°) 
op  — O ~ op  , o — o — o , op  — o,op  — 

* 2P 

Then,  by  substituting  this  result  into  the  previous  equation  for  V 


i2p  - fizp.op  x ^2p  + 4p,o  X v2p  - cf  <4  X a®  X R°) 


+ C2p  R® 
o - 


The  accelerometers  measure  f2p,  the  sum  of  inertial  accel- 
eration C2p  R°  and  gravitational  acceleration  G2p  Since  the 
0,  x x R term  is  dependent  only  on  position  it  can  be  combined 
with  G2p  to  form  j2p  = G2p  + C2p(s£f0p  x ^>op  x R°)  thereby 
yielding 


— 2p  - £2p  -i2p  + (£.2P,op  + 2^p,o>  * 't2* 
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This  then  yields  the  velocity  difference  equation 

v2P  - at  [f2P  -a2p  + ^2p,op  + 2s4p,o>  * 22p»  ♦ a™P 
* 

where  V ^ is  extrapolated  value  of  V^**  and  ’damp’  represents  the 
vertical  channel  damping  terms. 


The  altitude  h is  computed  as 


v* 

h (t)  = h (to)  + J dt  + damping. 


where  is  the  vertical  component  of  V p and 'damping*  represents 
damping  terms. 

Latitude,  longitude  and  alpha  angle  are  computed  from  the 
direction  cosine  matrix  ' DCM*  transforming  the  2P  frame  to  the 
OP  frame  (A  rotation  of  alpha  about  up,  latitude  abour  east,  and 
longitude  change  about  north) . The  differential  equation  for  the 
about  change  in  the  DCM  is: 


- c°P 

C2p 


C,*  x p 


2p 

op,  2p 


where  P0pf2p  is  the  angular  rate  of  the  2P  frame  with  respect 
to  the  OP  frame  as  coodinatized  in  the  2P  frame.  This  equation 
is  mechanized,  in  difference  equation  form,  as  a first  or  second 
order  DCM  update.  See  documentation  on  ANGVL2  for  an  explanation 

of  pop,2p* 

A third  order  vertical  damping  scheme  is  implemented  as 
follows  [the  inputs  are  the  AV's  from  the  vertical  accelerometer 
and  hb,  the  altitude  from  the  reference  (assumed  barometric) 
altimeter] : 
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Muf^i 


(NOTE  IMPLIES  UNIT  DELAY) 

Figure  5.  Vertical  Damping  Flow  Diagram 
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In  the  above,  DELT  is  the  navigation  cycle  time  and 
CV Dl,  DVD2  and  CVD3  are  damping  coefficients  whose  units  are 
gee”’1,  sec-2  and  sec  3 respectively. 

INRNAV  also  computes  THTTRQ , which  is  the  rate  of  the  2P 
(computational)  frame  with  respect  to  the  0 (inertial-initial 
earth  equatorial  Up-East-North)  frame.  This  rate  is  used 
(external  to  INRNAV)  as  follows: 

a)  Local  Level  - sent  as  "torquing  commands"  to  IMU 

b)  Space  stable  - unused 

c)  Strapdown  - used  to  "torque"  the  5 (body)  frame  to 
2P  (computational)  frame  DCM. 


B)  The  algorithms  used  by  INRNAV  are  selected  by  the  values 
of  IPC  (28)  , IPC  (29) , and  IPC  (30) , which  are  interpreted  as 
follows : 


^ Value 

Variable^" — 

0 

1 1 

IPC  (28) 

Use  first  order  DCM 

Use  second  order  DCM  j 

update  routine  for 

update  routine  for  | 

COP$2P 

COP$2P  1 

IPC  (29) 

Use  end  of  current 

Use  values  of  V$2P  % 

cycle  values  for 

ALPHA,  etc.  extrapo-  | 

V$2P,  ALPHA,  etc. 

lated  to  middle  of  ■ 

in  computations 

next  cycle,  in  compu- 

tations 

IPC  (30) 

Use  approximation 

Use  exact  formula  j 

in  computation  of 

in  computation  of  I 

angular  velocity, p 

angular  velocity, p 1 

.-.v  , IP.'S  A 


C)  In  broad  brush,  the  calculations  of  INRNAV  are  as 
follows : 

1)  Update  V2p  (stored  as  V$2P)  using: 

a)  velocity  increments  for  this  navigation  cycle 
(f2p  At)  (stored  as  DV$P) 

b)  the  (p  + 2fi) XV  At  term,  as  computed  on  the  previous 
cycle  (stored  as  WXV) 

c)  the  c[2p  At  term  (computed  by  multiplying  g$2P  by 
DELT) 

d)  the  damping  term  (computed  as  (VDMP  - CVD2  * DELH) 

* DELT) 

2)  If  IPC  (29)  = 1,  compute  extrapolated  value  of  velocity 

(stored  as  XV$2P)  and  interpolated  value  of  velocity  (.stored  as 
HV$2P) . 

3)  Compute  angular  rate  of  2P  frame  with  respect  to  OP 
as  follows: 

a)  If  IPC  (30)  - 0 

Compute  angular  velocity  using  approximate  formula 
(see  appendix  for  derivation) 

b)  If  IPC  (30)  = 1,  then 

i)  If  IPC  (29)  = 0 compute  angular  velocity 

using  exact  formula  with  velocity  at  end  of 
cycle  (V  $2P) 

ii)  If  IPC  (29)  = 1 compute  angular  velocity 

using  exact  formula  with  velocity  at  middle 
of  current  cycle  (HV$2P) 

4)  Update  CQP$2P  DCM  with  DCM  update  routine  according 

to  order  of  update  as  specified  by  IPC  (28) , computing  angles 
from  angular  velocity, If  IPC  (29)  = 1,  form  extrapolated  DCM 

X0P$2P.  Orthonomalize  C0P$2P  if  it  is  time  to. 
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5)  If  IPC  (29)  = 0,  complete  calculations  as  follows: 

a)  Use  TORCOR  to  compute  torquing  angles,  WXV  terms, 
based  on  end-of-cycle  values 

b)  compute  altitude  (stoned  as  H)  and  vertical  damping 
terms  DELH,  VDMP,  based  on  end-of-cycle  values. 

c)  Use  GRAV  to  compute  gravity,  based  on  end-of-cycle 
values. 

d)  Use  POSVEL  to  compute  position  and  velocity. 

6)  If  IPC  (29)  = 1,  complete  calculations  as  follows: 

a)  Compute  altitude  (stored  as  H) , extrapolated 
altitude,  vertical  damping  terms  DELH,  VDMP,  based 
on  extrapolated  values. 

b)  Use  GRAV  to  compute  gravity  based  on  extrapolated 
values. 

c)  Use  POSVEL  to  compute  position  and  velocity, 
extrapolated  alpha  angle. 

d)  Compute  angular  velocity  using  exact  formula, 
based  on  extrapolated  values. 

e)  Use  TORCOR  te  compute  torquing  angles,  WXV  terms, 
based  on  extrapolated  values. 


Ill 


INRNAV 


t 


COMPUTE  COMPONENTS  OF 
ANGULAR  VELOCITY  USING 
APPROXIMATION 


^USE\ 

^EXTRA- 

POLATED 

vVALUES> 

> r 


in 


USE  ANGVL2  TO  COMPUTE 
ANGULAR  VELOCITY  BASED 
ON  MIDPOINT  VELOCITY, 
ALPHA  ALTITUDE,  ETC. 


I USE  ANGVL2  TO  COMPUTE 
ANGULAR  VELOCITY  BASED 
ON  END-OF-CYCLE  VELOCITY 
ALPHA,  ETC. 


ROTATION  MATRIX 
STORED  IN  C$RHO 


USE  DCMUPD  TO  FORM  ROTA- 
TION MATRIX,  BASED  ON  ANG- 
ULAR VELOCITY,  TO 
SPECIFIED  ORDER 


ORDER  IS 
COMPUTED  AS 
1+IPCI28) 


PREMULTIPLY  BY  COPS2P 
TO  GET  ROTATION 
OF  COPS2P 


aagaasaasgaaBSa 


INRNAV 


/USE\ 

“^EXTRA- 

POLATED 

sA/ALUEV 


USE  POSVEL 
TO  EXTRACT 
POSITION  AND 
VELOCITY 


RETURN 


INRNAV 


USE  TORCOR  TO  COMPUTE 
TORQUING  ANGLES,  WXV 

T 

COMPUTE  C$RHO 


COMPUTE  G$2P  USING 
COPS2P,  S2PHI,  H 


l 


USE  POSVEL  TO  COMPUTE 
POSITION,  VELOCITY,  ETC. 


RETURN 


POSVEL 


2.3.6  Function  & Equations 


A)  The  purposes  of  this  subroutine  is  to 


a)  calculate  longitude,  latitude,  and  alpha  angle 


b)  compute  sine  and  cosine  of  alpha,  and  extrapolate 


values  of  the  sine  and  cosine  of  alpha 


c)  compute  velocity  in  the  2 frame  (Up-East-North) 


B)  The  CQp$2P  matrix  is,  analytically: 


s4>sacl  - co-sl 


-cas<j>cl  - sasl 


s <J®asl  + cacl 


-cac<!>sl  + sacl 


-sac  <t> 


ca  c <f> 


where  c <j>,  s 4>  are  the  cosine  and  sine  of  latitude  ca,  sa  are  the 
cosine  and  sine  of  alpha,  and  cl,  si  are  the  cosine  and  sine  of 
the  change  in  longitude  since  TIMEO.  Therefore 


LAT  = tan 


TEMP 


(>/(c<J>)  * t (sa)  c + (ca)*] 


- c*"'1  <§!> 


(C0P$2P  (3,2)) 2 + (C0P$2P (3 ,3) ) 2 


i)  if  TEMP  = 0,  longitude  and  alpha  cannot 


be  extracted  from  the  JDCM  so  that  LONG  and 


ALPHA  are  not  computed. 


ii)  if  TEMP  f 0/  longitude  and  alpha  can  be 
extracted  as  follows: 


■ 


I.PlflWilWWUmw'wm 


LONG 


+ LONGO 


= tan'1  (samssi-a ) 

\cor?2P  (i,i)/ 

- tan"1  (l^r)  + “>N50 

= /an-1  + L0!'G  0 

where  LONGO  is  longitude  at  time  = TIMEO.  Alpha  is  computed 
as  follows: 

ALPHA  = tan'* 

■ ta"_1  (fH)  - *“_1 1 (g) 

C)  Extrapolated  values  (to  the  middle  of  the  next  nav 
cycle)  of  the  sine  and  cosine  of  alpha  (XSALF  and  XCALF , respec 
tively)  are  computed  as  follows: 

DALPHA  = (ALPHA  -OALPHA)  /2 


where  OALPHA  is  previous  value  of  alpha. 

XSALF  = SALF  * DALPHA  * CALF 

XCALF  = CALF  - DALPHA  * SALF 

(these  are  the  first  order  approximations,  eg.  sin  (a  + at)  = 
sin  (a)  cos  (at)  + cos  (a)  sin  (a  t) 

= sin  (a)  + cos  (a)  at  + 0 ((at)2) 

■ALPHA-OALPHA 


a is  being  approximated  as 
DELT 


t is 


DELT 

,this  yields  above  approximation) 


D)  V$2  (the  velocity  in  the  2 frame)  is  computed  by 
rotating  V$2P(2)  and  V$2P(3)  through  the  alpha  angle  yielding 
V$2  (2)  and  V$2(3) . 
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Inputs,  Outputs  & Internal  Variables 
Inputs: 


LONGO  - Longitude  at  TIMEO 
C0P.$2P  - Tranformation  DCM 
V$2P  - Velocity  in  2P  frame 


Outputs : 


LAT  - latitude 

LONG  - longitude 

ALPHA  - alpha  angle 

SALP  - sine  of  ALPHA 

CALF  - cosine  of  ALPHA 

XSALF  - extrapolated  sine  of  ALPHA 

XCALF  - extrapolated  consine  of  ALPHA 

V$2  - velocity  in  2 frame 


POSVEL 


m 


m 


POSVEL 


EXTRACT  COSINE 
OF  LATITUDE 


COMPUTE  LATITUDE 


SAVE  OALPHA,  SET 
DALPHA  TO  ZERO 


COSINE  or 
^LATITUDE  . 
\ 0?  / 


COMPUTE  LONGITUDE 
ALPHA,  DALPHA 


COMPUTE  SINE,  COSINE  OF  ALPHA; 
XSALF,  XCALF  (BASED  ON 
DALPHA;  V$2 


GRAV 


2.3.7  Function  & Equations 

The  purpose  of  this  subroutine  is  to  compute  the 
acceleration  of  gravity  (as  coordinatized  in  the  2P  frame)  and 
to  compute  the  square  of  the  sine  of  the  latitude.  These  cal- 
culations are  based  on  the  inputs  which  are  altitude  (measured  from 
the  standard  ellipsoid  earth)  and  a DCM  from  2P  to  OP  (which 
yields  the  position  on  the  earth) . 

The  sine  squared  of  latitude  is  computed  by  picking 
up  the  sine  o'f  latitude  out  of  the  input  DCM  (row  3,  column  1 
is  sine  of  latitude)  and  squaring  it. 

The  vertical  (geodetic)  component  of  gravity  is 

computed  as 

-(g  + sin2$  + g^  sin4  4) 

* (1  “ (gh  " gh*sin  ^ h + ghh  h2) 

where  g,  g ^ g^,  gh,  ghh  and  gh(J)  are  coefficients  in  the 
expansion  of  the  analytic  expression  of  gravity  for  an  ellipsoid 
earth.  The  constants  are  based  on  the  WGS72  survey. 

The  north  component  of  gravity  is  computed  as 

-g^  * h * cos  * sin  <J>  where  h and  <P  are  (as  above)  altitude 
and  geodetic  latitude,  and  g1  is  another  WGS72  constant.  The 
north  component  is  then  rotated  through  the  alpha  angle  to 
be  able  to  express  it  in  the  2P  frame.  These  calculations 
duplicate  those  of  PROFGEN. 

Inputs,  Outputs  of  Internal  Variables 

A OP $2P  - Input,  DCM  taking  2P  to  OP 

AH  - Input,  altitude 

G$2P  - Output,  gravity  vector 

AS2PHI  - Output,  square  of  sine  of  latitude  extracted 
from  A0P$2P 
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ANGVL2 

2.3.8  Function  & Equations 

The  purpose  of  ANGVL2  is  to  compute  RHO,  the  angular 
rate  of  the  platform  frame (2P)  with  respect  to  the  earth-fixed 
frame  (OP) . These  rates  are  computed  based  on  an  ellipsoid 
earth.  The  north  and  east  velocities  (VN  andVE)  are  computed 
by  taking  the  velocity  in  the  2P  frame  (as  passed  to  the  sub- 
routine) and  rotating  it  through  the  alpha  angle  (sine  and 
cosine  of  alpha  are  passed  to  the  subroutine) . 

The  angular  rates  about  east  and  north  are  computed  as: 


AH  + RESQ  * {y~zt 


<AS2f>Hf 


WN  = 


aH  + E0  * <1  -■  ESQ  *- aszPHi1 


radius  of  curva- 
ture of  earth  in 
plane  defined  by 
north  and  up  unit 
vectors 


radius  of  curva- 
ture of  earth  in 
plane  defined  by 
east  and  up  unit 
vectors 


Where  AH  is  the  input  parameter  representing  altitude,  AS2PHI 
is  the  input  parameter  of  the  square  of  the  sine  of  latitude. 

RO  is  the  equatorial  earth  radius,  RESQ  is  R43  * (1  - ESQ) , where 
ESQ  is  the  square  of  the  earths'  eccentricity,  and  where  WE 
and  WN  are  the  rates  (about  east  and  north)  needed  to  keep  the 
up  axis  vertical.  WE  and  WN  are  then  rotated  through  the  alpha 
angle,  so  that  RHO  is  eoordinatized  in  the  2P  frame. 
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Inputs/  Outputs  & Internal  Variables 


VLCTY  - 


ASALF  - 


ACALF  - 


AS2PHI  - 


RHO (2)  - 
RHO (3) 

TMP1 , - 


input  parameter  representing  velocities  in 
2P  frame 

input  parameter  representing  sine  of  alpha 
angle 

input  parameter  esenting  cosine  of 
alpha  angle 

input  parameter  representing  square  of 
sine  of  latitude 

input  parameter  representing  altitude 
east  component  of  VLCTY  (internal  variable) 
north  component  of  VLCTY  (internal  variable) 
Rate,  about  east,  of  2P  frame  with  respect 
to  OP  (internal  variable) 

Rate,  about  north,  of  2P  frame  with  respect 

to  OP  (internal  variable) 

radius  of  curvature  of  earth  in  east 

direction  (internal  variable) 

radius  of  curvature  of  earth  in  north 

direction  (internal  variable) 

angular  rate  of  2P  frame  with  respect  to  OP 

frame,  as  seen  in  2P  frame  (note  that  rate 

is  about  level  axes  only)  - output  variable 

internal  variables 


ANGVL2 


f ANGVL2  J 

nr 

COMPUTE  VE  AND  VN  BY 
ROTATING  VLCTY  THROUGH 
ALPHA 

I 

COMPUTE  RADII  OF 
CURVATURE 

I 

COMPUTE  'RHO'  IN  UEN 
FRAME 


I 

ROTATION  RHO  INTO  2P 
FRAME  THROUGH  ALPHA 
ANGLE 


3 


C RETURN ^ 


ATUDE 


2.3.9  Function  & Equations 

The  purpose  of  ATUDE  is  to  implement  that  part 
of  the  navigation  algorithm  which  extracts  the  attitude  angles 
(roll,  pitch  and  heading)  from  C2P$5  and  a knowledge  of  ALPHA. 

Analytically,  it  is  known  that 


C2P$5 


cy  * cz  - cy  * sz 

cx*  sz-sx*sy*cz  cx*cz+sx*sy*sz 
-sx*sz-cx*sy*cz  -sx*cz+cx*sy*sz 


sy 

sx  * cy 
cx  * cy 


where 


sz 

= 

sine 

(roll) 

cz 

= 

cosine  (roll) . 

sy 

= 

sine 

(pitch) 

cy 

= 

cosine  (pitch) 

sx 

= 

sine 

(yaw) 

dx 

= 

cosine  (yaw  ) 

roll  = 


tan 


-1 


(~cy*sz) 

cy*cz 


tan 


-1 


/ ~C2P$5 (1 > 2)  ) 
\ C2P$5  ( *•/ 1)  / 


pitch 


tan 


if 


sy 

(sx*cy)2  + 


(cx*cy) 


= tan 


-i  I 


C2P$5 (1,3) 
W(C2P$5(2,3)2  + (C2P$5(3,3)2 


yaw 


alpha  + tan  1 / sx*cy \ _ 

1 cx*cy  I 


alpha  + tan 


-1 


/ C2P$5  (2,3) \ 
\C2P$5i3,3)  / 
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C2P.$5 

ALPHA 


ETA{1)  - 
ETA  (2)  - 

ETA  (3)  - 


Internal  Variables 

DCM  taking  5 frame  to  2P  frame  (input) 

alpha  angle  associated  with  2 frame  to  2P 

frame  (input) 

roll  (output) 

pitch  (output) 

heading  (output) 


COMPUTE  ROLL, 
PITCH  AND  YAW 


RETURN 


OUTUNI 

2.3.10  Function  & Equations 

The  purpose  of  OUTUNI  is  to  perform  all  calculations  needed 
to  prepare  data  for  output  in  man  readable  form.  All  angles  are 
converted  to  degrees.  The  differences  for  output  are  found  by 
subtracting  simulator  values  from  PROFGEN  values.  PROFGEN  velocity 
is  converted  to  (Up,  East,  North)  local  vertical  north  (LVN)  frame 
for  output  as  follows: 


"V$2T(lf 

H 

O 

0 

1  

V$T(lf 

V$2T (2) 

= 

-SALF$T  -CALF$T  0 

V$T (2) 

,V$2T(3). 

. CALF$T  -SALF$T  0. 

_V$T  (3 ). 

The  variables  are  described  below.  The  transformation  matrix 
takes  into  account  the  change  in  axes  plus  the  need  to  rotate  aT 
degrees  in  the  opposite  direction  as  in  PROFGEN.  IT  is  formed 
as  shown  below: 


‘1 

0 

0 

'o 

0 

r 

0 

CALF$T 

-SALF$T 

0 

-1 

0 

.0 

SALF$T 

CALF$T. 

.1 

0 

0. 

Rotate  -c^  degrees  to  change  of  axes 
put  in  LVN  frame  fror.  NWU  to  UEN 

Input  Variables 

RADPER  - Factor  for  converting  from  radians  to  degrees. 
LAT$T  - Geodetic  latitude  from  PROFGEN. 

L0NG$T  - Geodetic  longitude  from  PROFGEN. 

ALF$T  - Wander  angle  (aT)  from  PROFGEN. 

LAT  - Geodetic  latitude  from  simulator. 

LONG  - Geodetic  longitude  from  simulator. 

ALPHA  - Wander  angle  (aT)  from  simulator. 
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ETA$T  - Attitude  angles  from  PROFGEN. 

ETA  - Attitude  angles  from  simulator. 

V$T  - Aircraft  velocity  (NWU)  in  LLWA  from  PROFCSN. 

V$2  - Aircraft  velocity  (UEN)  in  LVN  from  simulator. 

HB  - Aircraft  altitude  from  PROFGEN. 

H - Aircraft  altitude  from  simulator. 

SALF$T-  Sine  of  AFL$T. 

CALF$T-  Cosine  of  ALF$T. 


Internal  Variables 
None 

Output  Variables 

OLAT$T  - LAT$T  in  degrees. 

OLON $T  - LONG$T  in  degrees. 

OALFST  - ALF$T  in  degrees. 

OLAT  - LAT  in  degrees. 

OLONG  - LONG  in  degrees. 

OALF  - ALPHA  in  degrees. 

OETA$T  - ETA$T  in  degrees. 

OETA  - ETA  in  degrees. 

V$2T  - Aircraft  velocity  from  PROFGEN  transformed  to  LVO 
(UEN)  frame. 

jA  - Difference  in  altitude. 

DLAT  - Difference  in  geodetic  latitude. 

DLONG  - Difference  in  geodetic  longitude. 

DALF  - Difference  in  wander  angle. 

DV  - Difference  in  aircraft  velocity  in  LVN  frame. 

DETA  - Difference  in  aircraft  atitude. 
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OUTUNI 


OUTUNI 


CONVERT  LATITUDE, 
LONGITUDE,  AND  WANDER 
ANGLE  FROM  RADIANS  TO 
DEGREES 


ETA$T(3)»ETA$T(3)— ALFST 


CONVERT  ATTITUDE  ANGLES 
FROM  RADIANS  TO  DEGREES 


TRANSFORM  PROFGEN  VELOCITY 
TO  LVN  (UEN)  FRAME 


TAKE  WANDER  ANGLE  OUT 
OF  PROFGEN  YAW  ANGLE 


Vjrrrrrrl^~. — r .n  aniiinw  nmmi  (mihmukhi* amihiw >uiw^ikuaw>aii  ■ mmauuium- «*. 

» jl i..5 » .« 15 l <h-.v_  -!-■ — . -r  "-!■ 1 t 1 - -.r - ^ m**- 


PRINTR 


2.3.11  Function  & Equations 

Thw  purpose  of  PRINTR  is  to  output  the  star  lard  output  variables 
on  a periodic  basis.  The  standard  variables  are: 

Time 

Latitude  (geodetic) 

Longitude  (geodetic) 

Alpha  (wander  angle) 

Altitude 

Velocity  (local  vertical  north) 

Attitude 

The  operator  specifies  the  period  between  printout. 

Input  Variables 

INIT  - Signals  first  pass  through  simulator.  ’0"  on  first  pass. 
IPRINT  - Number  of  lines  already  printed  on  page. 

IPLIM  - Limit  on  number  of  printed  lines  per  page . 

ITITLE  - 80  character  operator  specified  title. 

IPAGE  - Page  number. 

TIME  - Simulation  time  (elapsed  time) . 

IPC  (25) - Operator  controlled  flag  used  to  control  output  of 

filtered  attitude  data.  "0"  value  gives  no  printout. 
Default  value  is  "0". 

FILATT  - Filtered  attitude  data  3x3  array  containing  the  angle, 
angular  rate,  and  angular  acceleration  for  roll, 
pitch,  and  yaw. 

The  following  list  is  grouped  by  threes,  the  first  variable  in  each 
group  is  from  PROFGEN . The  second  variable  is  from  the  simulator, 
and  the  third  is  the  difference  between  the  two. 

OLAT$T , OLAT,  DLAT  - Geodetic  1.  Mtude. 

OLON$T , OLONG,  DLONG  - Geodetic  longitude. 

OALF$T,  OALF,  DALF  - Wander  angle. 

HB,  H,  DH  - Altitude 

V$2T,  V$2 , DV  - Velocity  in  local  vertical  north. 

OETA$T , OETA,  DETA  - Atti-ude  angles. 
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PLTAPE 


2.3.12  Function- & Equations 

The  purpose  of  PLTAPE  is  to  create  a tape  suitable  for  use  as 
input  to  plotting  software.  The  data  is  output  in  fixed  length 
records  22  words  per  record,  containing  the  variables: 

Time 

Latitude 
Longitude 
Wand  r angle 
Altitude 

Velocity  (LVN  frame) 

Attitude  (roll,  pitch,  yaw) 

All  the  above  items  represent  the  difference  between  simulator 
values  and  PROFGEN  values.  The  operator  can  specify  the  output 
period  via  card  input  at  execution  time.  A copy  of  the  operator 
specified  title  is  output  as  the  first  record  on  the  tape. 

Input  Variables 

INIT  - Flag  which  indicates  fxrst  pass  (when  equal  0) 
through  simulator. 

IT 2TLE  - Operator  specified  title:  80  characters. 

TIME  - Elapse  t’me 

The  following  variables  give  the  difference  between  the  simulator 
generated  value  and  the  value 'from  PROFGEN, 

DLAT  - Geodetic  latitutde. 

DLONG  - Geodetic  longitude. 

DALF  - Wander  angle. 

OH  - Altitude. 

DV  - Velocity  in  local  vertical  north. 

Frame  (U,E,N). 

DETA  - Attitude  angles  (roll,  pitch,  yaw). 

Internal  Variables 
None 


POUT 


i 


2.3.13  Function  and  Equations 

The  purpose  of  POUT  is  to  give  the  user  the  capability  of 
printing  variables  other  than  those  from  the  standard  output  set. 
This  can  be  used  for  diagnostic  or  comparison  purpose.  Output 
is  selected  by  a number  which  is  passed  in  the  calling  sequence 
as  shown  below: 

CALL  DOUT  (J) 

The  variable  J is  used  in  a computed  GO  TO  in  order  to  select  the 
desired  output.  The  output  variables  and  the  J value  associated 
with  each  are  described  below.  J values  20  through  30  are  unused  by 
DOUT. 


Input  Variables 


IPRINT 

IPAGE 

WT 

ETA$P 

DV$2P 

V$2P 

RHO 

COP$2P 

THTERT 

THTTRQ 


- Number  of  lines  already  printed  on  page 

- Next  page  number. 

- Angular  rates  about  LLWA  axes  (J=l) . 

- Four  gimbal  angles  for  local-level  and  space 
stable.  Incremental  angles  from  gyros  for  strap- 
down  (J=2). 

- Delta  velocity  for  use  as  input  to  navigation 
equations  (J=3). 

- Instantaneous  ground  velocity  as  computed  in  the 
navigation  equations  (J=4) . 

- Angular  rate  of  craft  with  respect  to  earth 
fixed  freme;  about  level  axes  only  (J=5) . 

- Direction  cosine  matrix  for  transformations  from 
LLWA  frame  (2P)  to  earth  fixed  frame  (OP)  (J=6  or  7). 

- Rotation  of  earth  with  respect  to  inertial  space 
during  one  computation  cycle  (J=8) . 

- Commanded  torque?  rotation  need  to  keep  LLWA  frame 
(2 P)  level  (J=9) . 

- u x v terms  of  velocity  update  equation  (J=10) . 

- Vertical  velocity  damping  term  (J=ll) . 

- Gravity  vector  in  LLWA  frame.  (J=12) . 


« 


i 


wxv 

VDMP 

G$2P 
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C2P$5 

C0$5 

TIME 

Q2P$5 


Incremental  misalignment  angle  used  to  update 
misalignment  matrix  (J=13) . 

- Transformation  matrix  from  LLWA  frame  (2P)  to  space 
stable  frame  (10)  (J=14). 

- Transformation  matrix  from  body  frame  (5)  to  LLWA 
frame  ( 2 P)  (J=15) . 

- Transformation  matrix  from  body  frame  (5)  to 

inertial  frame  (0)  (J=16). 

- Elapsed  time  in  seconds  (J=17) . 

- Quaternion  representation  of  rotation  in  transformation 
from  body  frame  (5)  to  LLWA  frame  (2P)  (J=19). 


Internal  Variables 

TMPARR  - Elements  (3,2),  -(3,1),  and  (2,1)  of  (C2P$5) 
Printed  when  J=18. 


T. 


(C2P$5T) 


Output  Variables 

IPRINT  - Number  of  lines  already  printed  on  page. 
IPAGE  - Page  number  of  next  page. 
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DOUT 


sK 
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ININIT 


2.3.14  Function  & Equations 

The  purpose  of  ININIT  is  to  initialize  variables  used 
by  INRNAV  (which  implements  the  navigation  equations) . If 
"higher  order  algorithm"  (IPC (29)^0)  is  selected  those  vari- 
ables needed  by  the  "higher  order  algorithm"  in  INRNAV  are 
also  initalized.  The  equations  are  the  same  as  those  used 
by  INRNAV. 


Variables  initialized  are  as  follows  (*  implies  initial 
ization  only  when  IPC(2S)^0,  © implies  recomputed  by  POSVEL) 


I 

! 


* 

* 

* 

* 


©* 

©* 

* 


LONGO 

TIMEO 

LAT 

LONG 

ALPHA 

CALF 

SALE 

C0P$2P 

H 

OHB 

HB 

S2PHI 

V$2P 

HV$2P 

XH 

OV$2P 

XS2PHI 


XSALF 

XCALF 

XV$2P 


E 


5^- 

R 


1 


All  initialized  to 
TIMEO  PROFGEN  values 


All  initialized  to  TIMEO  PROFGEN  values 


Initialized  according  to  values  of 
IPC(JO),  I PC  (.29)  and  IPC  (28), 
identically  as  INRNAV  would  compute 


INIT  set  to  1 to  indicate  end  of  initialization 
OTIME  is  recomputed  if  orthonormalization  takes  place  during 
ININIT 

Internal  variables  (all  scratch):  I,  J,  DCM32S,  DEM33S 
CORR,  ONEM 

Inputs: 

A is  used  as  scratch 

Universal  constants  (common  block  UNVRSL)  are  used, 

INCOR 

DELT 


RHO 
C$RHO 
G $2P 
WXV 

THTTRQ 

THTCOR 

THTERT 

DALPHA 

V$2 
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ININIT 


ININIT 


£ 


USE POSVEL  TO 
COMPUTE  POSITION, 
VELOCITY,  ETC. 


I 


COMPUTE  RHO(2),  RHO(3) 

WITH  XV$2P,  EXACT  FORMULA 


I 


USE  TORCOR  TO  COMPUTE 
TORQUING  ANGLES,  WXV 


I 


SET  INIT  TO  1 
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CMINTG 


2.3.15  Function  & Equations 

A.  The  purpose  of  CMINTG  is  to  form  up  the  AV's  that  would 
be  sensed  by  the  accelerometers  (in  a SS,  LL  or  SD  IMU)  over  the 
time  period  between  samples  of  simulated  accelerometer  data.  The 
routine  also  quantizes  (makes  allowance  for'  the  finite  resolution 
of)  the  accelerometer  data.  In  addition  the  routine  checks  for 
discontinuities  generated  by  PROFGEN  and  takes  special  action 
when  detected.  The  input  to  the  routine  is  in  the  form  of 
instantaneous  sensed  accelerations  (specific  forces) . 


B.  We  want  AVx  = J ax(t)dt.  (Similarly  for  AV^  and  AVz) 

n is  passed  to  CMINTG  as  ICMCYL  (CMINTG  cycle  length) . a (t) 
are  the  instantaneous  accelerations.  At  is  BELTS. 

i)  For  ICMCYL  odd  AV  is  computed  using  the  trapezoidal 
rule,  that  is 


t-ttiAt 


a (t  + (i-l)At)  + (t  + iAt) 


>«] 


ii)  For  ICMCYL  even  AVX  is  computed  using  Simpson's  rule, 
that  is 


nAt[ax(t)  + 2ax  (t+At)  + 4ax  (t+2At)  + 2ax  (t+3At)  + . ... 
4a  (t  + (n-2) At)  + 2a„ (t+ (n-1) At)  +av (t+nAt)  1/  (3n-2) 


C.  The  above  yields  the  total  AV.  We  must  now  quantize 
(truncate  to  allow  for  finite  resolution  of  the  accelerometers) 


TEMP  = [_  AV  + R*  j 


(R*  are  residuals  from 
previous  quantization) 

[ xj  means  round  x down 
to  an  integral  number  of 
units) 


• p 
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R = AV  + R*  - TEMP 
A V = TEMP 

D.  PROFGEN,  under  certain  circumstances,  generates  discon- 
tinuities in  its  acceleration.  CMINTG  performs  the  following  check 
for  the  x-component  {similarly  for  y and  z) 

(ax(t)  - ax(t-At))/At  > TOLJRK 

If  this  is  true  (on  any  axis)  a (t)  is  set  equal  to  a_(t-At) 
and  a warning  message  is  printed. 
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Inputs,  Outputs  and  Internal  Variables 


A.  Inputs 


SF$TP  - previous  value  (that  is,  from  TIME-DELTS) 
of  acceleration  in  accelerometer  frame 
SF$T  - current  value  of  instantaneous  acceleration 
in  accelerometer  frame 
TOLJRK  - tolerance  for  'jerk' 

ICMCYL  - Number  of  DELTS  intervals  over  which  CMINTG 
is  integrating 
TIME  - simulated  time 

ICMCYP  - Number  (that  is  1 thru  ICMCYL)  of  the  interval  that 
CMINTG  is  currently  processing 
DV$P  - Integrated  value  of  SF$TS 

RESID  - Residual  value  from  previous  cycle 

VQUANT  - Velocity  quantization  units  in  ft/sec 
DELTS  - Sample  interval 


SFST 


TIME 

ICMCYP 


DV$P 

RESID 

VQUANT 

DELTS 


B.  Outputs 

SF$TP  - next  'previous  value' 

ICMCYP  - next  'number  of  interval' 

DV$P  - updated  'integrated  value  of  SF$T' 

RESID  - Residual  for  next  cycle 

C.  Interval  Variables 

I - index  scratch 


CMINTG 


ENTRY 


/ANY  0F\ 
'Z  AXES  HAVES 
UNACCEPTABLE 
S.  JERKS  / 


CHECK  REMAINING  AXES 


COMPENSATE  FOR  JERK 
BY  ALTERING  PREVIOUS 
VALUE  OF  ACCELERATION 


ICMCYL 

EVEN 


PRINT  MESSAGE 
WITH  AXIS, 
TIME 


TRAPEZOIDAL 

RULE 


SIMPSONS 

RULE 


ADD  ,5*lSF$T  + 
SF$TP)  TO  DV$P; 
SAVE  SFSTAS 
SFSTP 


r 

:ck  \ 


CHECK 


= ICMCYL 


II* 


ATTPIL 


2.3.16  Function  & Equations 

The  purpose  of  ATTFIL  is  to  perform  the  calculations  necessary 
to  implement  a filter  for  the  attitude  data.  The  filter  operates 
by  keeping  estimated  values  of  nine  quantities,  the  roll,  pitch, 
and  yaw,  and  the  first  and  second  derivatives  (with  respect  to  time) 
These  nine  values  are  known  as  the  "state  matrix-".  i*he  steps 
performed  are  as  follows: 

1.  Extrapolate  the  state  matrix  to  the  current  time  (the 
state  matrix,  on  entry  to  the  attitude  filter,  has  the 
values  of  the  state  matrix  at  the  end  of  the  previous 
filter  cycle)  as  follows: 


Hew 

Transition 

* 

Old 

0 <f>  \p 

1 At 

* 

0 <p  \p 

a <p  ip 

= 

0 1 At 

• • • 

0 <p  \p 

0 <J;  ip 

m m 

1 

o 

o 

h- 

1 

♦♦  M •• 

0 <j>  ip 

where  6,  <J>,  ip  and  At  are  roll,  pitch,  yaw  and.  attitude 
cycle. 

2.  Compute  residuals,  that  is,  difference  between  measured 
0,  $,  and  ip  and  extrapolated  values. 


re  - 

*0.  *] 

'8* 

r 

“■m 

<s> 

yr 

- 

* 

rm 

- 

'P 

*-  r J 

.<P. 

3. 


where  8r  and  8^  are  residual  roll  and  measured  roll? 


likewise  <J> 


Kr'  V Tm' 

For  each  of  roll,  pitch,  and  yaw  select  one  of  ten  columns 
out  of  a 3-by-10  matrix  of  "filter  gains".  These  columns 
are  ordered  so  that  moving  towards  the  right  (that  is, 
increasing  the  column  number)  yields  columns  that  have 


'P, 
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increasing  reliance  on  the  "history"  as  reflected  in  the 
values  of  the  first  and  second  derivatives.  The  details 
of  the  selection  process  are  as  follows: 

a)  Compares  the  residual  against  a limit  (stored  as  RSMAX) , 
if  greater  "move  left"  by  decreasing  the  column  index 
(One  such  index  is  associated  with  each  of  roll,  pitch 
and  yaw) . Otherwise 

S'-)  Compare^  the  magnitude  of  the  difference  between  the 
old  residual  and  the  new  residual  against  a limit 
(stored  as  DRSMAX) , if  greater  "move  left"  by  decreasing 
the  column  index,  otherwise 

c)  "Move  right"  by  increasing  the  column  index. 

If  an  index  is  at  column  10  and  wishes  to  "move  right", 
one  stays  at  column  10.  If  an  index  is  at  column  1 and  wishes  to 
"move  left",  one  stays  at  column  1,  but  must  clear  the  correspond- 
ing first  and  second  derivatives. 

4.  Update  the  "state  matrix"  with  the  residuals  by  using  the 
selected  gains. 


*0  $ </f 

Kie  Ki<i>  Kiijj 

*er  0 O' 

“0  (J>  \p~ 

0 <j>  If 

«•  •• 

K20  K20  K2<p 

o <f>r  0 

+ 

• • • • 

0 <p  ip 

_0  <f>  4>. 

-K30  K3<J>  K3^  - 

.0  0 il>r. 

.8  4> 

where  K1Q  K2Q  K3Q  are  gains  from  column  selected  for  0; 
likewise  for  K. . , K_ . . K_ . . K. . . K_ . . k_  _ 


Inputs,  Outputs  and  Internal  Variables 
Inputs : 

ICP(25)  - 0 implies  filter  has  not  been  requested.  1 implies 

filter  has  been  activated. 

IGAINP  - 3 element  array  containing  column  indices  for  each 

of  roll,  pitch  and  yaw  respectively 


RSMAX, 

DRSMAX 


- tolerances 


OFILTR  - 


FILATT  - 


Outputs : 

OFILTR 

IGAINP 

FILATT 


Old  filter  residuals,  that  is  filter  residuals 
from  previous  cycle 

Attitude  computed  by  simulator  the  attitude 

"measurement"  for  the  filter. 

contains  state  matrix  at  end  of  previous  cycle 


as  above 


Interval  Variables 


FILTR 
DTSQ2 
Ir  J,K 


- holds  extrapolated  state  matrix 
At  above 

- holds  filter  residuals  until  stored  in  OFILTR 

- At*At/2 

- index  scratch 


SDINIT 


2.3.17  Function  & Equations 

The  purpose  of  SDINIT  is  to  perform  initialization 
when  strapdown  IMU  is  selected  (IMUTYP  = 3) . The  sequence  of 
initializations  performed  is  as  follows  (note  that  much  initiali- 
zation is  performed  in  the  BLOCK  DATA  routine) : 

a)  INREC  is  called  . This  initializized  all  variables 
in  the  TRAJIN  common  block,  in  addition  to  all 
rates  (DELT,  DELTS,  etc.):  ICMCYL  is  initialized 

so  that  integration  cycle  is  the  attitude  cycle. 

b)  COP$2P  is  initialized. 

c)  C2P$5  is  initialized. 

d)  C0$5  is  initialized. 

e)  SDINTG  is  called  to  transform  the  initial  accelera- 
tion vector  (SF$T)  into  the  platform  frame;  this 
vector  is  then  stored  as  SF$TP. 

f)  An  initialization  message  is  printed. 

g)  ININiIT  is  called  to  initialize  the  navigation 
equations  (see  IN.INIT  documentation) . 

h)  ATUDE  is  called  to  get  initial  ETAs. 

i)  Attitude  predictor-filter  is  initialized. 

Inputs,  Output  & Internal  Variables 

All  variables  not  previously  initialized  are  init: 


i 

i 
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SDATUD 


2.3.18  Function  & Equations 

A)  The  purpose  of  SDATUD  is  to  form  the  "incremental 
gimbal  angles"  that  would  be  produced  by  a strapdown  IMU. 

These  three  angles  are  incremental  "body"  rotations  (as  opposed  to 
incremental  Euler  angles) , that  is,  they  are  the  projection,  onto  the 
body  frame  axes,  of  r (where  | r|  is  the  angle  of  rotation  about  a 
unit  vector^^  or  r is  the  rotation  implicit  in  going  from  the 
old  C0$ 5 matrix  to  the  new  C0$5  matrix) . The  incremental  angles 
are  then  quantized,  and  the  residuals  saved  for  next  cycle. 


I B)  i)  The  rotation  (in  DCM  form)  is  given  as 

I . 

R = (COS  SOLD)  ”"1  (C0$5NEW) 

j ii)  The  rotation  DCM  generated  by  a rotation  of  0 

about  a unit  vector  n (components  nlf  n2,  n3) 
is  given  analytically  as 


1 

1 

I 

\\  nln2  nln3* 

H* 

O 

o 

R = (1  -COS0) 

n2nl  n2n2  n2n3 

© 

to 

o 

o 

+ 

0 10 

< 

1 

n3nl  n3n2  n3n3 

0 0 1 

' 0 

”n3 

n2 

+ sin  0 

n3 

0 

"nl 

;n2 

nl 

0 

since  first  two  matrices  are 

.5*  (R  (3,2)  - R(2,3) ) 
.5*  (R  (1,3)  - R(3,l) ) 
.5*  (R  (2,1)  - R(l,2)  ) 


symmetric 


sin© 

nl 

(call 

this 

pl> 

sin© 

n2 

(call 

this 

p2> 

sin0 

n- 

(call 

this 

Po) 

3 


NOW  p^2+  p22+  P32  = ^s^n®^2  + n22  + n32^  = (.sin®52 

since  n is  unit  vector. 

iii)  One  can  then  find  sin0,  0,  and  n^,  n2»  n^ 
and  compute.rGn^ , 0n2,  an<*  ®n3  a^y 

the  required  results. 

C)  C0$5  is  found  by  computing  C0$2P  (stored  in  scratch  matrix) 
and  multiplying  by  C0P$5  (which  was  computed  by  SSINTG  and  stored 
in  C2P'$5T. 


(D)  Once  the  incremental  angles  are  computed,  they  must 

* . 

be  quantized  (if  necessary) . With  R representing  the  residuals 
from  the  previous  cycle,  and  [X]  implying  that  each  component 
is  to  be  rounded  down  to  an  integral  number  of  units. 

TEMP  = [0n  + R*] 

R = 0n  + R*  - TEMP 

0n  = TEMP 

E)  (C0$50LD)  1 is  assumed  equal  to  the  transpose  of 
C0,$50LD. 


Inputs,  Outputs  & Internal  Variables 


LONG$T 

LONGO 

TIME 

TIMEO 

WERT 

SLAT$T 

CLAT$T 

SALF$T 

CALF$T 

C0$5 


longitude  per  PROFGEN 
initial  PROFGEN  longitude 
current  simulated  time 
initial  simulated  time 
earth  rate 

sine  of  PROFGEN  latitude 
cosine  of  PROFGEN  latitude 
sine  of  PROFGEN  alpha 
cosine  of  PROFGEN  alpha 


on  input,  previous  value  of  CO $5 
on  output. next  value  of  C0$5 


these 

inputs 

used  in 
computation 
of  C0$2P 


ETA$P (1)  - X rotation  ) 

ETA$P (2)  - Y rotation  / outputs 
ETA$P{3)  - Z rotation  / 
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STH  - internal  variable  used  for  sin0  above 

TH  - internal  variable  used  for  0 above 

A matrix-  internal  variable  used  for  R above 

B matrix-  internal  variable  used  for  C0$5NEW  above, 

until  placed  in  CO  $5 

SF  - internal  variable  used  for  TH/STH 

TMP  vector  - internal  variable;  TMP(l)  is  used  to 
hold  change  in  longitude  from  initial 
inertial  position 


SDATUD 


COMPUTE  CHANGE 
SINCE  TIME0  OF 
INERTIAL  LONGITUDE 


FORM 

CURRENT 

C0$2P 


FORM  CURRENT 
CO$5  = CO$2P+C2P$5T 


FORM  ROTATION  DCM 
R = (CO$SOLD)T*COS5NEW 


THIS  IS 
C05SNEW 


i 


EXTRACT^  SIN0,n2SIN0 
n3  SIN0  FROM  R 


l 


COMPUTE  SIN0,  0,  0„- 
9n2-*n3 


r 


I 


QUANTIZE 

ANGLES 


SDINTG 


2.3.19  Function  & Equations 


The  purpose  of  SDINTG  is  to  take  PROFGENs  specific 
forces  (as  coordinatized  in  PROFGENs  platform  'frame)  and  rotate 
them  into  the  body  frame.  Mathematically 


f5  = cl  cl*  f2QP 

2p  2qp  - 


(where  2QP  is  PROFGENS  North-West-Up  platform  frame) 
is  formed  from  PROFGENS  roll,  pitch  and  yaw. 


The  c; 


Inputs,  Outputs  & Internal  Variables 

Inputs  ETA$T  - roll,  pitch  & yaw 

Inputs  & output  SF$T  - specific  force : input  in 

2QP,  output  in  5 

Scratch  variables  - vector  TMP  (internal)  (used  to 

hold  cl*  f 2QP) , array  A (used 


to  hold  C^p) 


HSTHTG 


HSTITTG 

2.3.20  Function  & Equations 

A)  The  purpose  of  HSINTG  is  to  perform  the  calcula- 
tions associated  with  the  strapdown  high  speed  delta  -V  integration 
that  would  be  implemented  in  an  avionics  computer.  This  inte- 
gration takes  the  delta  -Vs  from  the  accelerometers  (at  the 
attitude  cycle  rate) , transforms  them  from  the  frame  of  the 
accelerometers  (synonymous  with  the  body  frame)  to  the  computa- 
tional frame,  and  sums  them  over  the  navigation  cycle.  The 
output  of  this  routine  is  therefore  the  delta  -V,  over  the 
navigation  cycle,  in  the  computational  frame.  This  subroutine 
also  must  update  C2pat  the  attitude  rate. 

B)  The  exact  equation  relating  accelerations  in  the 
two  frames  is: 

a2p  - c2pa5 


bo  that 


t+DELT 


2P 

AV“  = 


-/ 


a2Pdt  = 


= / 


t+DELT 


cfa*  dt 


where  DELT  is  the  navigation  computations  cycle.  Therefore 

n t+i DELTA 

’ “2P  - E / cf  a5  at 

i=l  t+(i-l)  DELTA 

where  DELTA  is  the  attitude  eyle  and 
n = DELT/DELTA. 


Ill 
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Assjvning  the  a5  to  be  constant  over  an  attitude  cycle, 

it  call  be  shown  (see  the  appendices)  that  the  following  approxi- 

2 

mat ion  is  good  to  0 (A©  ) . 

AV2P  = j[C5^(t  + + |C5P(t+i  At>]j  i5/2 

where  C^P(X)  is  value  of  C^p  at  time  X 

C>  The  exact  differential  equation  defining  the  time  rate 
2P 

of  change  of  is 

;,2P  _ r2P  5 
C5  " Cs  x “2P,5 

where  aj^p  5 1:116  angular  rate  of  the  2P  frame  with  respect  to 

the  5 frame,  as  expressed  in  the  5 frame.  Now 


~2P,5 


^o,5  " ^o,2P 


5 ,-,5  2P 

~ %,5  “ C2P  ^o,2P 


5 5 

The  03  c (or,  more  accurately,  DELTA  u K)  is  the  physical 
— c,o  —o , 3p 

data  measured  by  the  strapdown  IMU's  gyros,  2p  is  computed 
by  the  navigation  equations. 

D)  HSINTG  can  be  used  either  of  two  methods  to  compute 
the  value  of  C2P$5,  as  follows: 

i)  If  IPC{27)  equals  zero,  the  new  value  of  C2P$5  is 
computed  by  using  the  DCM  update  routine,  DCMUPD. 

Or thonomal i z a t ion  of  C2P$5  is  performed  every  HSOINC 
seconds,  beginning  with  the  initial  value  of  HSOTIM. 
The  order  of  the  update  is  given  by  IPC{26)  plus  one. 


! I Hi 
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ii)  If  IPC (27)  equals  one,  the  new  value  of  C2P$5  is 
computed  by  using  a quaternion  scheme,  where 
the  quaternion  q^*  (stored  as  Q2P$5)  represents 
the  rotation  from  the  5 frame  to  the  2P  frame. 
Specifically: 

a)  The  quaternion  corresponding  to  the  rotation 
of  the  2P  frame  during  the  attitude  cycle 
P^*  (stored  as  PO,  PI,  P2,  P3)  is  formed  to 
first,  second  or  third  order,  depending  on  the 
value  of  IPC (26). 

The  following  formulae  are  derived  in  an  appendix: 

1)  First  order 

P = (I,  AX/2,  AX/2,  AZ/2] 

% 

where  AX,  AY,  AZ  are  the  X,  Y and  Z components  of  the  rotation 
(see  SDATUD  documentation) 

2 ) Second  order 


liil)2  + 

(#)2  + 

(i4j 

\i A 

YU 

UJJ 

P . II  — .TTTj 

3)  Third  order 

I"  f M 2 + 2 + ££  2 

P = h - L2 1 2 , CON  * ~ + XDTX, 


CON  * + XDTY,  CON  * + XDTZ 


where 


CON  = 1 - 


XDTX 

XDTY 

XDTZ 


l[(ff  -(^)^(^)2] 

i /azay*  ‘az*ay\ 

- 6 (—2“  ” 2 j 

i /axaz*  ax*az\ 

6 \ 2 " 2 / 

1 /axay*  ax*ay\ 

6 l 2 2 ) 

i 


AX*,  AY*  and  AZ*  are  previous  values  of  AX,  AY  and  AZ. 

However,  these  XDTX,  XDTY  and  XDTZ  terms  are  the  result  of 

* 

a ft  x fi  term  (see  DCMUPD) . Due  to  the  infinite  angular  accelera- 
tions of  PROFGEN,  XDTX,  XDTY  and  XDTZ  have  beer  set  equal  to 
zero. 

b)  The  quaternion  is  updated  by 


q*P*  = qsP  X PgP*  where 

* indicates  "end  of  attitude  cycle"  (new)  value  and  'X'  indicates 
quaternion  muliplication. 


Therefore  (dropping  frames  from  notation  and  indicating 
quaternion  components  with  subscripts) : 


r~  ~ 
q0 

f—  “"i 

P0  ~P1  _P2  ~p3 

o 

j 

P1  P0  P3  “P2 

ql 

J 

g2 

P2  "P3  P0  P1 

q2 

to* 

p3  P2  “P1  P0 

g3 

- 

- — 

~~ 
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c)  The  quaternion  is  normalized  every  HSOINC  seconds 
starting  with  the  time  indicated  by  the  initial  value  of 
HSTOTIM. 

A quatrpnion  is  normal  if  the  sum  of  the  squares, 
of  its  components,  is  equal  to  1.  We  therefore  want  to  multiply 
each  component  by  y,  so  that 

(Yq0>2  + (rqi)2  + (Y92»2  + (Y93>2  = 1 

This  imples 


Y = 


Jq02+  qx2  + q22  + q32 


(1  + (q02  + qx2  + q22  + q32  - d) 


2 2 2 

A first  order  approximation  (that  is,  assuming  qQ  + q * + q2  + 
q32  - 1 is  small)  to  y is 

i 1 /„  2,_2,_.2,_2 

1 “ 2 <*0  + ^1  + ^2  + ^3  ~ 

- 1.5  - .5*  (q02  + q/  + q22  + q32) 

2d*  . ?V>* 

d)  C * is  constructed  from  q*‘  according  as 


follows : 


„2p*  = 


1 — 2 (q2  "t  q3  ) 2 (q^q2  ~ q0q3^  2 ^3*^1  q0q2^ 


2 (qxq2  + qQq3)  1-2  (q^^2  + q32)  2 (q2q3  - q^) 

2 (q3ql  ' q0q2)  2 (q2q3  + q0ql>  1 ~ 2 (ql2  + q22) 


e)  After  the  new  C2P$5  has  been  computed  the  average 
of  the  new  C2P$5  and  the  old  (that  is,  from  the  previous  attitude 
cycle)  is  computed.  The  delta  -V's  in  the  body  frame  are  pre- 
multiplied by  this  average,  yielding  the  delta  -V  (for  the  current 
attitude  cycle)  in  the  computational  frame.  This  quantity  is 
then  added  in  to  the  total  for  the  current  navigation  cycled  (stored 
in  DV5$2P) . 

Inputs,  Outputs  & Internal  Variables 


Inputs : 
C2P$5 
THTTRQ  - 


DELT 

DELTA 

Q2P$5 

HSOTIM 


HSOINC  - 
ETA$P 


previous  value  of  C2P$5 

— 0^2p  * DELT'  DELT  is  nav  cycle;  ^g^2p  ‘*‘s 

rate  of  comp  frame  with  respect  to  inertial,  as 

computed  by  INRNAV 

Nav  cycle 

attitude  cycle 

previous  value  of  quaternion  . . 

next  time  to  perform  normalization  (orthonormali- 

zation) 

time  between  normalizations  (orthonomalizations) 
incremental  angles  sensed  by  strapdown  gyros 
during  an  attitude  cycle 


previous  values  of  5 * DELTA 


Outputs : 

C2P$5  - current  value  of  C2P$5 


"previous  values"  for  next  attitude  cycle 


Q2P  $5  - current  value  of  rotation  quaternion. 
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Internal  Variables: 

TMP  <4  c DELTA  rotation  of  2P  with  respect  to  5 frame  over  one 
— 2p/  5 

attitude  cycle 

PO  \ 

I components  of  quaternion  representing  rotation  of 
( 2P  frame  during  an  attitude  cycle 


TS02>  internal  scratch  variables 


EQUIVALENCEd  to  Q2P$5 


XDTX  \ 

XDTY  s Elements  of  third  order  quaternion  rotating  to  SI  x n 
XDTZ  3 


internal  scratch  variables 


ds  new  value  of  C2P$5  until  averaging  is  through 
ds  average  of  new  and  old  value  of  C2P$5 


HSINTG 


HSINTG 


COMPUTE  SECOND 
ORDER  SCAUR 
TERM 


'SECOND' 

ORDER 

VUPDATE> 


COMPUTE  FIRST 
OR  SECOND  ORDER 
VECTOR  TERMS 
OF  QUATERNION 


UPDATE  QUATERNION 
WITH  ROTATION 
QUATERNION 


r TIME  TON 
.NORMALIZE., 


HSINTG 


NORMALIZE 
QUATERNION 
COMPUTE  NEXT  f iME 


FORM  NEW 
C2PSS  FROM 
QUATERNION 


FORM  "AVERAGE"  C2P$S 
UPDATE  C2PS5  FOR 
NEXT  CYCLE 


USE  ORTHO  SUBROUTINE 
TO  ORTHONORMALIZE  C2PS5 
COMPUTE  NEXT  TIME 


TRANSFORM  DELTA-V'* 
INTO  COMPUTATION  FRAME 
AND  ADD  TO  RUNNING 
TOTAL 


RETURN 
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H SINT 2 


2*3’21  !!M»ction  & Equations 


of  the  dena-vfi-TPOS!.°f  HS1”2  iS  t0  °°0raiMte  transmission 
of  the  delta-V  information  from  the  high  speed  strapdown  delta-V 

“Y?  “ e,Uati0nS-  “ 1S  °alled  ^ ***. 

ZlZ'is  “ Pi°tS  Up  the  delta-V  from  DV5WP  (where 

“ is  accumulating  them)  places  it  in  DV$p  (from  which  xmonv 
ill  input  them)  and  then  clears  DV5$2P. 


Outputs  s Internal  Variables 
Input  & Outputs:  DV5$2P 
Output:  DV$P 


C HSINT2  J 

_ET 


MOVE  DV5S2P 
TO  DV$P  AND 
CLEAR  DV5S2P 


RETURN 


SOI! Mil  *8!  —T  V.- 


QNTIZ 


2.3.22  Function  & Equations 


The  purpose  of  QNTIZ  is  to  perform  the  quantization 

function  (eg.  an  accelerometer  may  generate  delta  -V's  as  multiples 
—6 

of  10  g-sec,  or  a gimbal  angle  resolver  which  generates  angles 
as  an  integral  number  of  arc  minutes) . The  input  parameters  to 
the  routine  is  the  vector  to  be  quantized-  If  the  unit  is  zero, 
no  quantization  is  done,  else  (for  each  component  of  the 
vector) s 


X - UNIT  * where  [y]  is  the  'greatest  integer 

in'  y (that  is,  value  rounded  down). 

* 

Inputs,  Outputs  & Internal  Variables 

ARRAY  - parameter  representing  input  vector;  output 
. values  also  returned  here 

UNIT  - parameter  representing  unit  in  which  to 
quantize 

I - internal  scratch  variable 


ORTHO 


1 

1 

f* 


T- 

& 

i 

a 

5. 

f 


i 


‘i 

t 

2 

| 


2.3.23  Function  S Equations 

A)  The  purpose  of  ORTHO  is  to  orthonomalize,  to 
first  order,  a 3 x 3 array  passed  to  it  as  input.  The  update  used 
is. 


ARRAY-*- ARRAY  - .5*  ARRAY  * (ARRAYT  * ARRAY  - I ) 

where  I is  the  identity  matrix,  ARRAYis  the  ARRAY  to  be  updated, 
and  ARRAYT  is  the  transpose  of  ARRAY. 

Inputs,  Outputs  & Internal  Variables 


ARRAY 

- parameter  rr»- 

renting  input  argument 

I,  J 

-*  index  scratci. 

A,  B 

- scratch  3x3 

matrices 
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MATVEC 

2.3.28  Name 

Matrix  Times  Vector  (MATVEC)  routine. 

Function 

This  subroutine  multiplies  any  3x1  vector  by  any  3x3  matrix. 

Operation  and  Equations 

Calling  sequence  is  given  below: 

CALL  MATVEC  (D,  B,  C) 

| 

where : 

D ^ 3 x 1 vector  result 
B = 3 x 3 matrix 
C - 3 x 1 input  vector 

D = [B]  C 

Only  vector  D is  modified  upon  return  to  the  caller. 

Input  Variables 

Defined  in  calling  statement. 

interna  1 Variables 

A - i :.  1 vector  to  collect  results  of  multiplications. 

Output  Variables 

Defined  in  calling  statement. 
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2.3.25  Name 


MTM 

Matrix  Transpose  Multiply  (MTM)  routine. 

Function 

This  subroutine  multiplies  any  3x3  matrix  by  the  transpose 
of  any  3x3  matrix. 

Operation  and  Equations 

Calling  sequence  is  given  below: 

CALL  MTM(A,B,C) 

where : 

[A]  = [Bf  [C] 

The  same  matrix  can  be  used  for  more  than  one  argument.  Only  matrix 
A is  modified  upon  return  to  the  caller.  The  detailed  equation 
is: 


*all 

a12 

a13~ 

1 

tr 

H 

H* 

b21 

b31 

*cll 

C12 

C13* 

a21 

a22 

a23 

= 

b12 

b22 

b32 

X 

C21 

C22 

C23 

-a31 

a32 

a33- 

-b13 

b23 

b33- 

-c31 

C32 

C33- 

Input  Variables 

Defined  in  the  calling  statement. 

Internal  Variables 

AA  - Used  to  collect  result  of  matrix  multiply. 


Output  Variables 

Defined  in  the  calling  statement. 
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2.3.26  Name 


Matrix  Multiply  (MM)  routine. 


Function 


Multiply  any  3x3  matrix  by  any  3x3  matrix. 


Operation  and  Equations 

Calling  sequence  is  given  below: 

CALL  MM(D,B,C) 
where : 

ID]  = [B]  [C] 

The  same  matrix  can  be  used  for  more  than  one  argument.  Only 
matrix  D is  modified  upon  return  to  the  calling  program. 


Input  Variables 


Defined  in  calling  statement. 


Internal  Variables 

A - Used  to  collect  result  of  matrix  multiply. 


Output  Variables 


Defined  in  calling  statement. 


2.3.27  Name 


ROTXYZ 


XYZ  Rotation  Matrix  (ROTXYZ)  routine. 

Function 

Forms  the  3x3  rotation  matrix  using  three  Euler  rotations. 

Operation  and  Equations 

Calling  sequence  is: 

CALL  ROTXYZ  (ARRAY,  SZ , CZ,  SY,  CY,  SX,  CX) 

where : 

ARRAY  = 3x3  rotation  matrix  returned  to  calling  routine 
SZ  = sine  of  Z rotation 
CZ  = cosine  of  Z rotation 
SY  = sine  of  Y rotation 
CY  = cosine  of  Y rotation 
SX  = sine  of  X rotation 
CX  = cosine  of  X rotation 

The  sequence  of  rotations  is  minus  Z degrees  about  Z,  minus  Y 
degrees  about  Y,  and  X degrees  about  X.  The  detailed  matrix 
equation  is  given  below: 


all 

a12 

a13 

1 

0 

0 

cosY 

0 

sinY 

cosZ 

-sinZ 

0 

a21 

a22 

a23 

= 

0 

cosX 

sinX 

0 

1 

0 

sinZ 

cosZ 

0 

a31 

a32 

a33 

0 

-sinX 

cosX 

-sinY 

0 

cosY 

0 

0 

1 

— 

« 

. 

Where  the  a matrix  is  returned  to  the  caller  as  the  variable 
ARRAY. 


* 


i 
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Input  Variables 

Defined  in  calling  sequence. 

Internal  Variables 

SYCE  = SY*CZ 
SYSZ  = SY*SZ 

where  SY,  SZ,  and  CZ  are  defined  in  the  call  statement 
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2.3.28  Name 

Matrix  Transpose  (MATRAN) 

routine 

Function 

Transpose  any  3x3  matrix. 

Operation  and  Function 

Calling  sequence  is  given  below: 

CAL  MATRAN  (B, A) 
where : 

[B]  = [A]T 

A and  B must  be  two  different  3x3  matrices. 

Input  Variables 

Defined  in  calling  sequence 

Internal  Variables 
None. 

Output  Variables 

Defined  in  calling  sequence. 


2.3.2.29  Name 


ROTZYX 


' ! 


ZYX  Rotation  Matrix  (ROTZYX)  routine. 


Function 

Forms  a 3x3  rotation  matrix  using  3 Euler  rotations.  Se- 
quence of  rotations  is  X degrees  about  X,  Y degrees  about  Y,  and 
Z degrees  about  Z. 

Operation  and  Equations 

Calling  sequence  is  given  below: 

CALL  ROTZYX  (ARRAY,  SZ,  CZ,  SY,  CY,  SX,  CX) 
where : 

ARRAY  - 3x3  rotation  matrix  returned  to  calling  routine 
SZ  - sine  of  Z rotation 
CZ  - cosine  of  Z rotation 
SY  - sine  of  Y rotation 
CY  - cosine  of  Y rotation 
SX  - sine  of  X rotation 
CX  - cosine  of  X rotation 


The  detailed  matrix  equation  is  given  below: 


*11 

a12 

a13 

cosZ 

sinZ 

0 

cosY 

0 

-sinY 

1 

0 

0 

a21 

a22 

a23 

= 

-sinZ 

cosZ 

0 

0 

1 

0 

0 

cosX 

sinX 

a31 

a32 

a33 

0 

0 

0 

sinY 

0 

cosY 

0 

-sinX 

COSX 

Where  the  a matrix  is  returned  to  the  caller  as  the  variable  ARRAY. 


Input  Variables 

Defined  in  the  calling  sequence. 

Internal  Variables 

SYSX  = SY*SX 
SYCX  = SY*CX 


\ 
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Output  Variables 


Defined  in  the  calling  sequence. 
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DCMUPD 


2.3.30  Function  S Equations 

The  purpose  of  DCMUPD  is  to  perform  as  a utility 
subroutine  which  can  form  up  a matrix  representation  of  a "small" 
rotation  of  a "DCM"  (Direction  Cosine  Matrix) . DCMUPD  can  make 
a first,  second,  or  third  order  approximation  to  the  exact  form  of 
the  update  matrix  (see  the  accompanying  analytic  discussion  for 
an  explanation  of  'order*  and  the  derivation  of  the  formulas  used 
by  this  subroutine) . DCMUPD  can  be  called  in  such  a way  that 
the  resulting  output  is  either 

a)  A rotation  matrix,  that  is  the  updated  DCM  can 
be  formed  by  taking  the  old  DCM  and  post  multi- 
plying the  output  of  DCMUPD  (the  diagonal  entries 
of  such  a .matrix  are  1 nominally ' 1) 

b)  A matrix  which,  when  pre-muliplied  by  the  old 
DCM,  yields  the  change  (in  the  additive  sense) 
to  the  old  DCM.  (the  diagonal  entries  of  such 
a matrix  are  'nominally'  0) 

The  inputs  to  DCMUPD  are 

i)  DX,  DY,  DZ  - components  of  rotation  vector, 

that  is  y = DX*  + DYA  + dza  represents  a rotation 
x y z 

of  |y|  radians  about  the  unit  vector  which  is 
Y 

III ' 

ii)  DIAG  the  'nominal'  value  of  the  diagonal  entries 

iii)  IORDM:l  - one  less  than  the  order  of  the 
update 

iv)  ODX,  ODY,  ODZ  - previous  values  of  ODX,  ODY  and 
ODZ*  (see  footnote) 

For  a first  order  update,  the  following  formula  is  used 
(DCM  is  the  output  of  the  routine ) . 


i 


'DIAG 

-DZ 

DY  ' 

DZ 

DIAG 

-DX 

-DY 

DX 

DIAG 

For  a second  order  update,  the  following  matrix  is  added 
on  to  the  first  order  results 


DY**2  - DZ**2 

I DX*DY 

I DZ*DX 

DX*DY 

j -DZ**2  - DX**2 

1 DY*DZ 

j 

DZ*DX 

« DY*DZ 

[ -DX**2  - DY**2 

For  a third  order  update,  the  following  matrix  is  added 
to  the  sum  of  the  first  and  second  order  matrices* 


l 

-2*Q*DZ 

1 -2*Q*DY 

0 

• 

-DY*ODX 

' -DZ*ODX 

1 

+0X*ODY 

+DX*ODZ 

-2*Q*DZ 

i 

i 

1 -2*Q*DX 

+DY*ODX 

1 

\ 

0 

I -DZ*ODY 

-DX*ODY 

1 

\ 

' +ODY*DZ 

1 

-2*Q*DY 

\ 

1 

-2*Q*DX 

t 

1 

+DZ*ODX 

) 

+DZ*ODY 

f 0 

-DX*ODZ 

f 

1 

-DY*ODZ 

1 

where  Q = 2 + (DX**2  + DY**2  + DZ**2) 

* The  terns  including  ODX,  ODY  and  ODZ  are  part  of  the 
third  order  update  only  and  arise  from  an  angular  acceleration 
term.  Due  to  the  step  changes  in  PROFGEN's  angular  rates,  these 
terms  have  been  omitted  from  the  subroutine. 

Inputs,  Outputs  & Internal  Variables 

The  inputs  were  described  above. 

The  outputs  are  DCM  (the  DCM  update  matrix)  and  ODX 
ODY,  & ODZ,  which  are  set  equal  to  DX,  DY  and  DZ  (respectively) 
just  prior  to  exit  and  are  to  be  used  during  the  next  update  of 
the  DCM. 

Internal  variables  SI,  S2,  S3,  Tl,  T2,  T3  and  SCL. 
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LLINTG 

2.3.31  Function  S Equations 

The  purpose  of  LLINTG  is  to  transform  the  specific  forces 
from  the  PROFGEN  platform  frame  to  the  local-level  platform  frame. 
If  the  effects  of  torquing  are  not  considered,  the  transformation 
is  given  below 


,2'  _ 2'  f2Q' 

- “ C2Q*  - 


0-10 


2Q'  denotes  PROFGEN  platform 
frame 


2 ' denotes  local-level  wander 
azimuth 


When  torquing  commands  from  the  navigation  equations  are  introduced, 
the  platform  can  become  misaligned  from  the  true  local-level  wander 
azimuth  frame.  These  misalignments  must  be  found  and  used  when 
calculating  the  specific  force  sensed  by  the  platform.  The  necessary 
calculations  are  as  follows: 

1.  Transform  velocity  from  PROFGEN  platform  (2Q')  to  local 
vertical  north  (2). 

V2  = C2  v20' 


= -sinan 


a,p  - PROFGEN  wander  angle 


2.  Calculate  radii  or  curvature 


re (1_e  } 

^ h (1-  e2  sin2L) ^ 


-MWK*- WW  JWt  ftWnUW  IW* 


where 


P 


= h + 


(1-  e sin  L) 


r - Meridian  radius 
m 

r - Prime  vertical  radius 
P 

h - Altitude  above  sea  level 
rg  - Sea  level  equatorial  radius 

e - Earth  eccentricity 
L - Geodetic  latitude 

3.  Compute  the  angular  rates  due  to  the  velocity  and  earth 
rate  then  transform  to  actual  platform  frame 


o)„  « w sinL  (rotation  about  vertical) 
u e 

UL  = -V  /r  (rotation  about  east) 

E n m 

u>N  = Vg/rp  + cosL  (rotation  about  north) 


0), 


T 


2M' 

^2' 


(0 


where : 
0) 


Earth  angular  rate 

Angular  rate  in  local  vertical  north 


frame  = 


to, 


U 


0). 


N 


- 


V„  - 


Aircraft  velocity  in  north  direction 
Aircraft  velocity  in  east  direction 


C 


2 

2 


I 


10  0 
0 cos«T  sinaT 

0 -sinaT  cosaT 


t 
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= Transformation  from  LLWA  frame  ( 2 * ) to  the  mis- 
aligned LLWA  frame  (2M1)  calculated  on  previous 
cycle  {see  below) . 

= Angular  rate  in  2M'  frame  due  to  trajectory  velocities. 


Update  the  misalignment  matrix  (C^,  ).  The  torque  com- 
mands are  quantized  in  order  to  represent  the  resolution 
of  an  actual  torquer.  The  commands  are  then  combined  with 
the  angular  rates  calculated  in  step  3 to  give  the  angular 
error  due  to  the  difference  between  commanded  orientation 
and  LLWA  orientation.  These  misalignment  angles  are  put 
into  a skew  symmetric  matrix  which  is  used  to  update  the 
misalignment  matrix. 

a = e^m  - em 

E QT  T 


where : 
9, 


2M1  _ 2M' 

2'  ~ 2' 


Ajigular  error 

Angle  traversed  by  IMU  during  one  sample  time 
due  to  quantized  commanded  torque 

Angle  traversed  by  IMU  during  one  sample  time  due 
to  aircraft's  velocity  over  earth 


” 1 0E3 

"0E2 

-eE3  1 

9ei 

6E2  ~eEl 

1 

Specific  force  is  transformed  from  the  PROFGEN  platform  frame 
(2Q 1 ) 'to  the  misaligned  LLWA  frame  (2M') 

f 2 ' _ r2M'  r2'  2Q' 

- C2,  c.2Qf  £_ 
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f - Specific  force  in  indicated  frame. 

Input  Variables 

CALF$T  - Cosine  of  wander  angle  from  PROFGEN. 

SALF$T  - Sine  of  wander  angle  from  PROFGEN. 

V$T  - Velocity  vector  from  PROFGEN  in  north,  west,  up  frame. 

ESQ  - Earth  eccentricity  squared. 

SLAT$T  - Sine  of  geodetic  latitude  fron  PROFGEN 

HB  - Altitude  from  PROFGEN 

* 

Ed  - Earth  equatorial  radius 

RESQ  - R0*ESQ 

THTTRQ  - Commanded  torque  vector:  angle  to  be  traversed  during 
the  navigation  cycle. 

TQUANT  - Torquing  angle  quantization. 

DELT  - Navigation  cycle  period. 

IPC(l)  - Flag  for  operator  controlled  printout  of  the  angular 
rate  due  to  aircraft  velocity  (WT) . If  0 no  print- 
out. Default  is  0. 

IPC (13) - Flag  for  operator  controlled  printout  of  misalignment 
angles.  If  0 no  printout.  Default  is  0. 

SF$T  - Specific  force  vector  from  PROFGEN  in.  North,  west,  up 
frame. 

IOPNLP  - Flag  to  control  whether  or  not  misalignments  due  to 

torquing  are  to  be  calculated.  0 mean  calculate  mis- 
alignments. Default  is  0. 


Internal  Variables 

VN  - Aircraft  velocity  in  north  direction. 

VE  - Aircraft  velocity  in  east  direction. 
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TMP1 

TMP2 


Scratch  variables  for  calculating  radii  or 
curvature. 


J 

RP  - Prime  vertical  radius  of  curvature. 

RM  - Meridian  radius  of  curvature. 

WT  - Angular  rate  of  aircraft  due  to  aircraft  velocity 

over  the  earth  in  LLWA  frame. 


WTE  - Angular  rate  of  aircraft  about  east. 

WTN  - Angular  rate  of  aircraft  about  north. 

TRESID  - Residual  torque  angle  generated  as  a result  of 
quantizing  the  torque  command. 

h - 3x3  skew  symmetric  matrix  representing  the  incremental 

misalignment  angles 

TMP  - Temporary  (scratch)  vector. 


Output  Variables 

2m  * 

C$MIS  - 3x3  misalignment  matrix.  Referred  to  as  C2,  in  above 
description. 

SF$T  - Specific  f rce  vector  in  LLWA  or  misaligned  LLWA 
frame. 


LLINTG 


r 


f 


r 


i 

{ 

* 

i 

t 
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2.3.32  Func'tion  & Equations 

The  purpose  of  LLATT  is  to  implement  those  calculations  that^ 

would  be  performed  in  an  airborne  computer  in  constructing  the  Cg 

direction  cosine  matrix  (transformation  from  body  (5)  frame  to 

2 ' 

local-level  (21)  computational  frame).  The  Cg  matrix  is  con- 
structed from  the  four  gimbal  angles  generated  by  the  LLtyTUD  routine. 
The  order  in  which  the  angles  are  used  is  given  below: 

Outer  Roll  (negative  about  Z) 

Pitch  (negative  about  Y) 

Inner  Roll  (negative  about  Z) 

Yaw  (positive  about  X) 


Two  intermediate  matrices  are  formed  by  using  the  subroutine  ROTXYZ 
and  then  multiplying  these  two  matrices  to  give  Cg  . The  equations 
are  as  follows: 


where: 


C 


2' 

5 


3" 

Cg  transforms  through  minus  outer  roll  and  minus 
pitch 


2’ 

C-wj  transforms  through  minus  inner  roll  and  yaw. 


' cosXcosW 

-cosXsinW 

sinXl 

s 

sinW 

cosW 

0 

rsinXcosW 

sinXsinW 

cosXj 

cosY 

-sinY 

0’ 

= 

cosZsinY 

cosZcosY 

sinZ 

r-sinZsinY 

-sinZcosY 

sinZ. 

where: 

W - Outer  roll 
X - Pitch 
Y - inner  roll 
Z - Yaw 
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saw  t 


Input  Variables 


ETSSP  - outer  roll,  pitch,  inner  roll,  yaw  gimbal  angles. 
Equivalence^  to  W,X,Y,Z  respectively. 


Internal  Variables 


A - 3x3  array  which  corresponds  to  C^,!  above. 


B - 3x3  array  which  corresponds  to  ct"  above 

5 * 


Output  Variables 


C2P$5  - Transformation  matrix  from  body  to  local-level  frames 


Corresponds  to  above. 


- vr  -■'v *E  - V- 


LLATUD 


2.3.33  Function  & Equations 

The  purpose  of  LLATUD  is  to  generate  the  four  gimbal  angles 
that  would  be  generated  by  a local-level  IMU.  Moving  from  the 
body  frame  (5)  to  the  local-level  frame  (21),  the  order  of 
rotations  is: 

Outer  Roll  (negative  about  Z) 

Pitch  (negative  about  Y) 

Inner  Roll  (negative  about  Z) 

Yaw  (positive  about  X) 

where : 

Positive  roll  is  right  wing  down. 

Positive  pitch  is  nose  up. 

Positive  yaw  is  counterclockwise  about  down. 

Subroutine  ROTXYZ  is  used  to  form  the  body  to  local-level  trans- 
formation matrix  (C2P$5).  This  is  then  multiplied  by  the  misalign- 
ment matrix  (C$MIS).  C$MIS  represents  the  difference  between  IMU 
orientation  calculated  from  PROFGEN  data  and  the  IMU  orientation 
caused  by  torquing  commands  from  the  navigation  equations.  If  a 
perfect  platform  is  being  simulated,  C$MJS  is  set  to  identity. 

Once  C2P$5  has  been  formed,  the  gimbal  angles  can  be  extracted  as 
follows: 


ETA$P (1) 
ETA$P (2) 


= tan 


-1  feSIW-]  - 


= tan 


-1 


C2P$5 (1,3) 


*^2P$5  (2, 3)  2 + C2P$5  (3 ,3)  2 


= pitch 


ETASP (3)  - tan’1  [g|g||{|;|j]  = yaw 


After  the  above  angles  have  been  quantized  or  truncated  to  give  the 
operator  specified  resolution,  ETA$P(4)  is  set  to  ETA$P(3)  since  ETA$P(3) 
is  actually  the  inner  roll  angle.  ETA$P(3)  is  then  set  to  zero. 
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Input  Variables 

ETA$T  - Roll/  pitch,  and  yaw  from  PROFGEN. 

C$MIS  - Misalignment  matrix  which  transforms  the  C2P$5  matrix 
from  the  perfect  local-level  frame  as  generated  by 
PROFGEN  to  the  local-level  frame  generated  as  a result 
of  torguing  commands  from  the  navigation  equations. 

AQUANT  - Gimbal  angle  quantization  term. 

IPC(2)  - Flag  which  controls  operator  selectable  printout  of 

ETA$P . If  zero,  no  printout.  Default  is  no  printout. 

Internal  Variables 

TEMP  - Temporary  variable  to  hold  the  square  root  of  C2P$5(2,3) 
squared  plus  C2P$5(3,3)  squared. 

C2P$5  - 3x3  rotation  matrix  from  body  to  local-level  wander 
azimuth  (2')  frame.  Includes  torquing  misalignments 
if  calculated. 

Output  Variables 

ETA$P  - Four  local-level  wander  azimuth  gimbal  angles.  Order 
is  outer  roll,  pitch,  inner  roll,  yaw.  Inner  roll  is 
always  zero. 


V 


LLATUD 


LLATUD 


ROTXYZ 


C§  = CM|S  C5 


EXTRACT 

3GIMBAL 

ANGLES 


ETA$P(3)  IS  SET  TO  YAW  ANGLE 
BECAUSE  QNTIZ  ACCEPTS  ONLY 
3 ELEMENT  VECTORS 


QNTIZ 


ETA$P(4)=ETA$P{3) 

ETA$P(3)=0.0 


IPC(2)=0 


SIMULATES  PRECISION  OF 
ANGLE  RESOLVER 


DOUT(2) 

PRINT 

ETASP 


RETURN 


LLINIT 


\m 


m 


2.3.34  Function  & Equations 


The  purpose  of  LLINIT  is  to  perform  the  initialization 
needed  by  the  local  level  IMU  model  and  the  navigation  equations 
for  local  level  input.  For  the  most  part  this  initialization 
is  performed  by  a series  of  calls  to  the  appropriate  subroutines 
(see  flow  charts).  The  only  calculations  performed  in  LLINIT 
are  as  follows: 

ICMCYL  = ITRNAV* ITRATT 
where : 

ICMCYL  = Number  of  trajectory  input  cycles  which  must  be 
integrated  between  each  navigation  cycle. 

ITRNAV  = Number  of  attitude  calculation  cycles  per  navigation 
cycle. 

ITRATT  = Number  trajectory  inputs  per  attitude  cycle. 

The  second  calculation  establishes  the  first  past  value  of  the  specific 
forces  which  are  input  from  PROFGEN . The  specific  forces  are  trans- 
formed from  the  PROFGEN  frame  (North,  West,  Up)  to  the  simulator 
local  level  frame  (Up,  East  North). 


SF$TP(1)1  0 0 

SF$TP (2 ) =0-1 

SF$TP  (3)  1 0 


SF$T (1) 
SF$T  (2) 
SF$T (3) 


where : 

SF$TP  = Past  value  of  specific  force  in  local-level  frame 
(U.E.N. ) . 

SF$T  = Present  value  of  specific  force  as  input  from  PROFGEN 
(N.W.U. ) . 

Input  Variables 
ITRNAV  ) 

ITRATT  ? See  above  for  definition 
SF$T  ' 

IOPNLP  - Control  flag  to  establish  whether  or  not  torquing 


— raj 


misalignment  within  the  local-level  IMU  model 
is  to  be  calculated.  0 = calculate  misalignment; 
1 = no  misalignment. 


Internal  Variables 


None 


Output  Variables 


ICMCYL 

SF$TP 


See  above  for  definition 


SSI NIT 


2.3.35  Function  & Equations 

The  purpose  of  SSINIT  is  to  perform  initialization 
when  space  stable  IMU  is  selected  (IMUTYP  = 2) . The  sequence  of 
initializations  performed  is  as  follows  (note  that  much  initiali- 
zation is  'performed*  in  the  BLOCK  DATA  routine). 

A)  Compute  C®q  rotation  based  on  rotations,  in  the 
Euler  angle  sense,  of  SSXO  about  X,  followed  by 
SSYO  about  Y,  followed  finally  by  SSZO  about  Z. 

B)  Compute  Cq°  by  transposing  C®Q 

OP 

C)  Initialize  Cg  to  identity 

D)  INREC  is  called.  This  initializes  all  variables 
to  all  the  rates  (BELT,  DELTS,  etc) : 

E)  ICMCYL  is  initialized  sc  that  the  CMINTG  inte- 
gration cycle  is  the  navigation  cycle. 

OP 

F)  C2p  is  initialized 

G)  SSINTG  is  called  to  rotate  the  TIMED  specific 
force  into  the  space  stable  frame,  which  is 
subsequently  stored  in  SE$TP. 

H)  An  initialization  message  is  printed 

I)  SSATUD  is  called  to  compute  the  initial  gimbal 
angles. 

J)  SSATT  is  called  to  compute  the  initial  C2P$5 

K)  ININIT  is  called  to  initialize  the  navigation 
equations . 

L)  ATUDE  is  called  to  extract  the  gimbal  angles 

M)  The  attitude  filter  is  initialized,  if  necessary. 
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Inputs , Outputs  and  Internal  Variables 


All  variables  not  previously  initialized  are  initialized. 


SSINIT 


PRINT  INITIALIZATION 
MESSAGE 


SSATUD 


2.3.36  Function  & Equations 

The  purpose  of  SSATUD  is  to  compute  the  gimbal  angles 
that  would  be  generated  by  a perfect  space  stable  IMU.  The  IMU 
is  assumed  to  be  a four  gimbal  platform,  with  the  rotations 
about  Z,  X,  Y and  Z,  in  that  order,  going  from  the  body  in  towards 
the  IMU.  The  Y rotation  is  assumed  to  be  zero,  so  that  the 
rotation  is  a Z X z rotation. 

The  first  step  is  to  form  the  C2P'$5  matrix  by  using  the 
ROTXYZ  routine  and  the  roll,  pitch  and  yaw  angles  from  PROFGEN. 

A Z(a^)  X (a3)  Z (a^)  rotation  can  be  written  as  follows  (Wi.ere 
S^  = sine  (a^) , = cosine  (a^) , S3  = sine  (a3) , C3  = cosine  (a^) 

= cosine  (a4) : 


A = 


' C1C4 

- S1C3S4  \ 

C1S4 

+ S1C3C4 

• S1S3* 

i 

-S1C4 

- C1C3S4  ! 

t 

-S1S4 

+ C1C3C4 

1 C1S3 
| 

. S3S4 

i 

• 

-S3C4 

; C3  . 

The  angles  (a^»“3r  and  «4)  are  there 
follows : 


r 


1.  Compute  S,  as  \(A  (3,1)) 2 + (A  (3  2)) 2 


2.  If  S3  is  not  ^0,  compute 
.-1 

|ac 

s. 


cu 


= tan 


= tan 


= tan 


/A  (1 , 3)  \ 

\A(2'3)/ 

| A (3 ,3) j 

1 / A(3,l)  \ 
C-A  (3,2)  / 


We  wish  to  keep  «3  between  0°  and  180°  (to  avoid  a situation 
known  as  "gimbal  flip").  Therefore  if  either  or  a4  has  changed 
by  more  than  II/2,  do  the  following 
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ijgsijgEZjiij^gp  tsSS^jr 


$:  | 


m i 
fr  5 


ot4  88  a4  + H 


a3  a “a3 


a^  = a^  + II 


To  allow  for  the  way  the  outer  gimbal  is  usually  mounted 
subtract  H/4  from  a. 

Now  quantise  the  attitude  angles  in  units  of  AQUANT,  using 
the  QNTIZ  subroutine. 


Inputs,  Outputs  & Internal  Variables 
Inputs : 

ETA$T  - Roll,  pitch  and  yr.w  from  PROFGEN 
C10$2P  - tranformation  from  computational  to  space 
stable  frame  (computed  each  DELTS  by 
SSINTG) 

AQUANT  - angle  quantization  units 
Input  and  Output: 

ETA$ P - Old,  on  input,  values  of  SS  gimbal  angles, 
Output  as  new  values. 

Internal  Variables : 

THl,  TH2,  TH3  - EQUIVALENCEd  to  ETA$P(1)  etc. 

A - holds  C10$5 

0TH2,  OTHl,  DTH1,  DTH2  - internal  scratch  variables 


SSATUD 


C10$S  STORED 
IN  A 


SAVE  IN 
OTH1,  OTH2 
RESPECTIVELY 


COMPUTE  S3 


SAVE  IN 
TEMP 


EXTRACT  Ctj,  o^, 
a4  FROM  C10$5 


HOLD  IN 
TH1.TH3, 

TH2 

RESPECTIVELY 


COMPUTE  CHANGE  IN 
®l'a4 


DTH1,  DTH2 
RESPECTIVELY 


SSTFRM 

2.3.37  Function  and  Equations 


The  purpose  of  this  routine  is  to  implement  that 
portion  of  the  flight  code  (of  a space  stable  system  only)  which 
would  transform  the  delta  -V’s  from  the  space  stable  to  the 
computational  frame.  This  routine  is  always  called  just  prior 
to  INRNAV.  The  basic  equation  is: 

f2P  _ r2P  pOP  0 f 10 

- ~ t'QP  '“O  10  - 


14 


m 


is  set  up  at  initialization  and,  with  the  assumption 

of  a perfect  space-stable  IMU,  remains  constant  throughout  time. 

Cq  is  just  the  rotation  matrix  resulting  from  the  rotation  of 

the  earth  since  TIMED;  it  is  computed  to  the  middle  of  the 

2P 

current  navigation  cycle.  CQp  is  computed  as  follows: 

OP 

i)  If  IPC (29)  ■ 1 X0P$2P  contains  C^p  at  the  middle 

of  the  current  navigation  cycle;  the  transpose  of  X0P$2P  is  used 

ii)  Otherwise  (that  is  IPC (29)  = 0),  Cgp  is  computed 

as  follows: 


= C 


1 „0P  „ 2P 

2 C2P  X £ 0P,2PAt 


where  C^p  is  the  value  (as  computed  by  INRNAV  and  stored  in  C0PS2P 
of  the  beginning  of  the  current  navigation  cycle,  and  jD?Pp  2p 
(as  computed  by  INRNAV  and  stored,  in  skew-symetric  matrix  form 
as  C$RHO)  is  the  angular  rate. 


Inputs,  Outputs  & Internal  Variables 
Inputs : 

DV$P  - delta  V 

C0$10  - transformation  from  space  stable  to 
'TIMEO  equatorial  Up-East-North ' 
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WERT  - 
TIMEO  - 


earth  rate 
initial  TIME 


TIME  - current  TIME 
DELT  - navigation  cycle 

OP 

X0P$2P-  middle  of  cycle  C^p 


CSRHO  - 
C0P$2P- 


Outputs: 


skew  symetric  form  of  RHO*DELT 

OP 

beginning  of  cycle  C2p 


DV$P  - delta  V's,  now  in  computational  frame 
C0P$0  - DCM  from  0 to  OP  frame 

Internal  Variables: 

WET  - rotation  of  earth  since  TIMEO 

A - holds  1/2  C2p  x £ oP,  2P 
op 

b - holds  c:; 


.TEMP 

TEMP2 


- scratch  vectors 





SSTFRM 


ROTATE  SPECIFIC 
FORCE  INTO  O FRAME 


COMPUTE  C|JP 


I PC  (29)  *»  0 


FORM  Cjp  X fiJp(2P 


FORM  Cqp  BY  TAKING 

TRANSPOSE  OF  THE 
SUM  OF  THE  ABOVE 


PLUS  Cgp 


ROTATE  SPECIFIC 
FORCE  INTO  OP  FRAME 


V 

IPCI29)  = 1 


FORM  BY 
TRANSPOSING  XOPS2P 


ROTATE  SPECIFIC  FORCE 
INTO  2P  FRAME 


I 


STORED 
IN  TEMP 


RETURN 


SSINTG 


2.3.38  Function  & Equations 

The  purpose  of  this  routine  is  to  transform  the 
specific  forces  from  the  PROFGEN  platform  frame  to  the  space 
stable  frame;  as  such  it  is  part  of  the  ’ simulator.  The 
equation  used  is  as  follows: 

f10  _ -10  r2P  f2QP  (2QP  is  PROFGENS 

~ ' 2P  2QP  ' platform  frame) 

10  , 2P 

the  routine  also  forms  C2p  which  is  used  by  SSAT'iJD . The  C2gp 

tranformation  consists  only  of 

i)  interchanging  the  first  and  third  components  and 

ii)  negating  the  second  component. 

C2p  is  computed  as 

10  _ 10  co 

l2P  ~ ^0  2P 

C^is  available  as  an  input;  it  represents  the  orientation 
of  the  platform  in  inertial  space,  and  as  such,  does  not  change 
with  time. 

C2p  is  the  transformation  from  platform  to  "equatorial 
Up-East-North  at  TIME#".  It  is  a rotation  of  alpha  about  up 
(note  that  PROFGENs  alpha  has  the  opposite  sign  convention) , 
followed  by  a rotation  (in  the  Euler  angle  sense)  of  latitude  about 
east,  followed  by  a minus  rotation  of  "longitude  change"  about 
north.  "Longitude  change"  is  computed  as 

WERT  * (TIME  - TIME)?)  + LONG $T  - LONG# 

where 

WERT  is  earth  angular  velocity  in  radians/sec 
TIME  is  current  (end  of  cycle)  time 
TIME0  is  initial  time 
LONG0  is  longitude  at  TIME# 

L0NG$T  is  current  (end  of  cycle)  longitude  as  computed  by  PROFGEN 


Inputs,  Outputs  & Internal  Variables 


! 


I 

| 


I 


Inputs : 


WERT 
TIME 
TIME# 
LONG# 
LONG  $T 


C10.$0 
SLAT$T 
CLAT  $T 
SF'$T 


1 sine  and  cosine  of  latitude  as  computed 
} by  PROFGEN 

PROFGENS  specific  forces  in  2QP  frame 


Ouputs : 

C10S2P 

SF$T  Specific  force  in  space  stable  frame 


SSINTG 


COMPUTE  “LONGITUDE  CHANGE" 


USE  ROTZYX  TO  FORM 
COS2P 


. 

m-- 

t 

ms 


R 

ft 

' 


* 

TRANSFORM  SPECIFIC  FORCE 
FROM  2QP  TO  2P  FRAME 

» 

“ } 

MATRIX 

MULTIPLY 

COMPUTE  C10S2P  - C10S0*CO$2P 

1 

MATRIX  TIME 

VECTOR 

MULTIPLY 

TRANSFORM  SPECIFIC  FORCE 
FROM  2P  TO  10  FRAME 

BY  SFST  - C10$2P*TEMP 

r , 

( RETURN  ) 
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CO$2P 
STORED  IN 
A 


STORED 

NOWIN 

TEMP 


( 


i 


i 


2.3.39  Function  & Equations 

The  purpose  of  this  routine  is  to  implement  those 
calculations  that  would  be  performed  by  an  airborne  computer  in 
constructing  the  C2p$5  DCM  (transformation  from  body  to  compu- 
tational frame).  The  transformation  C10$5  (from  body  to  space 
stable  frame)  can  be  constructed  by  a rotation  of  (a^  + ir/2)  about  Z, 
followed  by  (in  the  Euler  Angle  sense)  a rotation  of  a 3 about  X, 
followed  in  turn  by  a rotation  of  about  Z.  The  ct^,  a3  and 
are  the  girnbal  angles  read  in  from  a space  stable  IMU. 


A Z(a^)  X (a^)  Z (a^)  can  be  written  as  follows  (where 
sine  (a^) , = cosine  (a^) , S3  - sine  («3) , C3  = cosine  (a3) / 

sine  (a^) , C4  = cosine  (a^) . 


C1C4  - S1C3S4  CIS 4 + S1C3C4  S1S3 


-S1C4  - C1C3S4  -S1S4  + C1C3C4  C1S3 


j^S3S4  -S3C4  C3  J 

Once  we  have  CIO $5  we  can  use  C0P$2P,  C0$10  and  C0P$0 
(which  are  available  as  inputs)  as  follows: 

C2P$5  = [C0P$2P]T  * [C0P$0]  * [C0$  10]  * [C10$5] 


Inputs,  Outputs  & Interval  Variables 
Inputs : 

C0P$2P  - DCM  from  2P  to  OP 
C0P$0  - DCM  from  0 to  OP 

C0$10  - DCM  from  10  to  0 

ETA^P  - girnbal  angles  (generated  by  SSATUD) 


| 

I 

1 
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Outputs 

C2P  $5  - DCM  from  5 to  2P 
Internal  Variables 

A - holds  C2P  $0/  then  C2P$1P 

B - holds  CIO  $'5 

SI,  Cl,  S3  J 
C3,  S4 , C4  ( 

V Scratch  variables  used  in  creating 
S1S4 , C1C3  {.  Cl  0$5 

S3S4,  C1C4  I 


2.3.40  Function  And  Equations 

The  purpose  of  this  subroutine  is  perform  rounding 
(optional)  and  truncation  of  floating  point  numbers ; this  is 
necessary  to  simulate  an  airborne  computer  whose  precision  is 
less  than  that  of  the  'host'  computer  (the  host  is  the  computer 
on  which  the  simulation  is  being  run,  a CYBER  74) . For  a detailed 
description  of  the  algorithms,  refer  to  the  comments  in  the  listing. 

Inputs,  Outputs  & Internal  Variables 

Input  argument  - remember,  this  is  an  assembly  language 
subroutine 

FRACTL  - length  of  fraction  being  simulated 

ROUFD  - odd  implies  rounding,  even  implies  trunca- 

tion only 

Output  result 


AS  DEFINED 
BYCDC 

HARDWARE  T 


FORM  ROUNDING/ 
TRUNCATING  WORDS 


TAKE  CORRECTIVE 
ACTION 


/* \ 


R-.URN 


VPINIT 


2.3.41  Function  & Equations 

The  purpose  of  this  routine  is  to  initialize  the  precision 
varying  routine  with  information  concerning  the  length  . of  the 
exponent  of  the  floating  point  numbers  to  be  simulated.  For  a 
more  complete  description,  refer  to  the  comments  in  the  routine 
listing  in  Volumu  IV. 

Inputs,  Outputs  & Internal  Variables 

Input  - EXPOL  - length  of  exponent 
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YSin 


2.3.42  Function  & Equations 


The  purose  of  VSIN  is  to  compute  the  sine  of  a single  argument. 
VS IN  implements  the  sine  routine  that  would  be  implemented  in  an 
airborne  computer.  The  algorithm  to  be  used  is  selected  by 
specifying  SINPTH  to  be  1,  2,  3 or  4;  these  algorithms  are  designed 
to  produce,  respectively,  24,  32,  40  or  48  hits  of  accuracy  in 
the  result.  VSIN  sets  ar.  indicator,  and  then  calls  VSINCO  (a 
subroutine  that  it  shares  with  VCOS)  to  perform  the  actual  com- 
putations;. For  a detailed  description  and  flowchart,  see  VSINCO 
documentation,  section  2.3.4  4. 

Inputs,  Outputs  & Internal  Variables 

ARG  - ii.put  argument 

VSIN  - result 


2.3.43 


The  purpose  of  VCOS  is  to  compute  the  cosine  of  a single 
argument.  VCOS  implements  the  cosine  routine  that  would  be 
implemented  in  an  airborne  computer.  The  algorithm  to  be  used  is 
selected  by  specifying  COSPTH  to  be  1,  2,  3,  or  4;  these  algorithms 
are  designed  to  produce  respectively,  24,  32,  40  or  48  bits  of 
accuracy  in  the  result.  VCOS  sets  an  indicator,  and  then  calls 
VSINCO  (a  subroutine  that  it  shares  with  VSIN)  to  perform  the 
actual  computations.  For  a detailed  description  and  flowchart, 
see  VSINCO  documentation,  section  2.3.44. 

Inputs,  Outputs  & Internal  Variables 

ARG  - input  argument 

VCOS  - result  aalue 


VSINCO 

2.3.44  Function  & Equations 

The  purpose  of  this  routine  is  to  compute  the  sine  or  cosine 
of  an  input  argument.  Sine  vs.  cosine  is  selected  by  an  input 
argument,  and  the  algorithm  is  likewise  selected  by  an  input 
argument.  This  routine  is  used  by  both  VSIN.  and  VCOS;  VSINCO 
implements  the  calculations  that  would  be  performed  by  the  air- 
borne computer. 

The  algorithm  input  argument  will  have  the  value  1,  2,  3 or 
4 which  will  select  an  algorithm  that  is  accurate  to,  respectively, 
24,  32,  40  or  48  bits. 

It  appears  that  most  SIN/COS  routines  are  implemented  . 
either  as  polynomials,  or  as  ratios  of  polynomials.  These 
polyonomials  are  evaluated  over  a reduced  range  as  follows: 

Assume  X is  the  argument,  as  expressed  in  radians. 


Z = £ abs  (X) 
TT 

r = z-q 


q = gii  (z) 


If  VSIN  was  requested,  and  X<0  add  4 to  q 
If  VCOS  was  requested  add  2 to  q 


Let  qQ 
Let  q^ 
Let  q2 


= q mod  8 
= gii  (Qq  /4) 
= q^  mod  4 


Then  the  result  (for  either  VSIN  or  VCOS)  is  as  follows: 

for  q^  = 0 (l)ql  sin  (^  r) 

for  q2  = 1 (-l)gl  cos  (1-r) 

for  q2  — 2 (-l)gl  cos  (v-  4) 

for  q2  = 3 (-l)ql  sin  (|  (1-r) 


JiJiiLI.il,. 


Simplifying  further 


rl  = 


for  q-  mod  2=0 


1-r  for  q2  mod  2=1 


_ 0 for  q2  = 0,3 


1 for  q2  = 1,: 


Then  the  result  is 


for  q4  = 0 (-l)ql  sin  r.i) 


for  q4  = 1 {-l)ql  cos  (j  r^ 


Various  expansions  (both  polynomial  and  ratio  of  polynomials 


are  available  for  sine  (5-  rl)  0 < rl  <1  (likewise  for  cos  (5-  rl) . 


are  avarraore  tor  sine  n;  u ^ n iiiKewise  ror  cos  ri)  . 
Several  of  these  are  listed  in  the  reference  ("  Computer  Approxi- 
mations", Hart  et  al  QA297,C738). 


Assuming  the  relative  execution  times  as  follows: 


the  expansions  for  sin  (^-  r^)  compare  as  follows: 


Index 

Precision  (in 

Relative 

in  Reference 

decimal  digits) 
(relative) 

Execution 

3040 

8.49 

12 

3041 

11.34 

16 

3042 

14.35 

20 

3043 

17.48 

24 

3044 

20.73 

28 

3045 

24.07 

32 

3060 

8.63 

22 

3061 

11.36 

26 

3062 

14.62 

30 

3063 

17.59 

34 

3064 

21.13 

38 

3065 

24.29 

42 

We  desire 

routines  that  will  produce 

answers  accurate  to 

24,  32,  40  and 

48  binary  digits  (approximately  7.23,  9.61,  11.! 

and  14-37) . 

With  the 

same  assumptions,  the  cosine 

expansion  compare  i 

follows: 

Index 

Precision  (in 

Relative 

in  Reference 

decimal  digits) 
(relative) 

Execution 

3820 

7-49 

12 

3821 

10.25 

16 

3822 

13.18 

20 

3823 

16.25 

24 

3824 

19.45 

28 

3825 

22,74 

32 

3840 

7.54 

22 

3841 

10.10 

26 

3842 

13.34 

30 

3843 

16.18 

34 

3844 

19.71 

38 

3845 

22.78 

42 
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Therefore  define  paths 

as  follows: 

I 1 

PATH 

cos 

Expansion 

SIN 

Expansion 

Accuracy  in 

Decimal  Digits 

; I 

(relative) 

; If 

1 

382G 

3040 

7.49 

; 1 

2 

3821 

3041 

10.25 

' s 

3 

3822 

3042 

13.18 

1 

4 

3823 

3043 

16.25 

1 

Inputs,  Outputs,  and  Internal  Variables 


X 

Q 


IPATH 


input  argument 

0 implies  we  want  sin  abs  (x) 

2 implies  we  want  cos  abs  (x) 

4 implies  we  want  -sin  abs  (x) 
algorithm  to  use  (1,  2,  3 or  4) 


| 
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VSIN/VCOS  FLOWCHART 


VSINCO 


VATAN2 


m 


■m 


2.3.45  Function  & Equations 


The  purpose  of  VATAN2  is  to  compute  the  arc  tangent  of 
two  arguments,  that  is,  tan  (^) , VATAN2  implements  the  arctangent 
routine  that  would  be  implemented  in  an  airborne  computer.  The 
algorithm  to  be  used  * - selected  by  specifying  ATNPTH  to  be  1,  2, 

3,  or  4;  these  algorithms  are  designed  to  produce,  respectively, 
at  least  24,  32,  40  or  48  bits  of  accuracy  in  the  result.  The 
only  error  condition  is  if  both  Y and  X are  zero;  in  such  a case 
the  standard  Fortran  arctangent  error  routine  is  invoked. 

VATAN2  takes  two  arguments  Y and  X,  which  can  be  graphically 
represented 

Y 

where  0 is  the  arc  tangent  and  - II<0  < n 

A 

if  X > 0 VATAN  2 ( Y|  X)  sgn*  (Y)  atan  ( I Y I , j X |.) 

where  atan  (|  Y |.,  | X [)  = tan"1  (|  Y |/|  X |)  and  the 

principal  value  is  between 
0 and  ^ 

if  X < 0 VATAN 2 (Y,  X ) = sgn* (Y)  (n  - atan  ( | Y | , j X | ) ) 

if  | X | > f Y | atan  ( | Y | , | X | ) = tan-1  ( | Y j / | x | ) 

if  | Y | < | X j atan  (|  Y |,  | X |)  - J -tan"1  (|  X \/\  Y i) 


* sgn  is  1 or  — 1 *-|  X | is  the  absolute  value  of  X 
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The  argument  of  tan”1  is  between  0 and  1,  Various  further 
range  reductions  are  possible;  one  of  them  does  not  require  any 
table  searches.  The  algorithm  is  as  follows; 


if  arg  < tan 


JI 

12 


= 2 


-V5* 


then  result  is  between  0 and 


if  arg  > tan 

then  tan”1  (arg)  = jr  + tan 
where  the  argument  satisfies 


“l|  [V?  - II  arg  1 + arg 


arg-t^T 


n n 


- tan  < tan  < tan  X2 

The  reference  (."Computer  Approximations”,  Hart,  et.al., 
QA297.  1738)  lists  several  expansions  for  this  range. 

Assuming  relative  execution  times  as  follows; 


ADD 

MOL 

DIV 


1 

3 

10 


The  expansions  are  as  follows; 


m 


Index 

Reference 

Precision  in 

Decimal  Digits  (relative) 

Execution 

Time 

4940 

7.69 

8 

4941 

9.54 

12 

4942 

11.38 

16 

4943 

13,20 

20 

4944 

15.01 

24 

4945 

16.82 

28 

4946 

18.63 

*5  ^ 

5050 

6.62 

18 

5051 

8.98 

22 

5052 

11.34 

26 

5053 

13.70 

30 

5054 

16,06 

34 

5055 

18.42 

38 

5056 

20.78 

42 

5057 

23.14 

46 

5058 

25.51 

50 

We  desire  routines  that  will  produce  answers  accurate  to 
24,  32,  40  and  48  binary  digits  (approximately  7.23,  9.61,  11.99 
and  14.37  decimal  digits). 

There rore  we  will  define  the  following  paths: 


Path 


Precision  in 

Decimal  Digits  (relative) 


Index 

in  Reference 


1 

2 

3 

4 


7.69 

11.39 

13.20 

15,01 


4940 

4942 

4943 

4944 


Inputs,  Outputs  & Internal  Variables 


XARG 
YARG 
Al’NPTH 
VAT AN 2 


Two  input  arguments 

Selects  "algorithm'1 
Returns  value 


•fS 
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i*u 


VATAN2 


a 
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VATAN2 


is: 


VSQRT 


2.3.46  Function  & Equations 

The  purpose  of  VSQRT  is  to  compute  the  square  root  of  a 
single  argument.  VSQRT  implements  the  square  root  routine  that 
would  be  implemented  in  an  airborne  computer.  The  algorithm  to 
be  used  is  selected  by  specifying  SQRPTH  to  be  1,  2,  3 or  4: 
these  algorithms  are  designed  to  produce,  respectively,  at  least 
24,  32,  40  or  48  bits  of  accuracy  in  the  result.  The  only 
error  condition  is  if  the  argument  is  less  than  zero;  in  such 
a case  the  standard  Fortran  square  root  error  routine  is  invoked. 
An  assembly  language  subroutine,  VSQSUP  is  used  to  perform  range 
reduction,  as  explained  hereinafter. 

I)  Algorithms  are  generally  of  the  form 

1)  Make  an  initial  approximation 

2)  Apply  some  number  of  iterations  of  Newtons  method 

Steps  1)  and  2}  are  related  in  that  the  better  the  initial 
approximation,  the  fewer  the  number  of  iterations  are  needed. 

In  the  most  general  sense,  the  selection  of  a "optimal" 
routine  is  a function  of; 

a)  space  vs.  speed  considerations 

b)  relative  speeds  of  add,  multiply,  divide 

c)  required  precision 

II)  Initial  approximations 

The  first  step  in  making  an  initial  approximation 

is  to  reduce  the  range  of  the  argument  (eg.  from  10**  -65  >1.) 

The  stated  reduction  seems  the  most  likely  to  be  implemented  (on 
an  avionics  system)  since  alternatives  rely  on  additional  program 
instructions  and  constants.  This  range  reduction  (on  a binary 
floating  point  computer)  could  be  implemented  by  simply  examining 
the  exponent  of  the  original  argument.  Stated  mathematically  • 
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REDUCT  « GII  (EXPO/2) , where  EXPO  is  exponent  of 
NEWARG  and  GII  is  "greatest  integer  in0  eg,  GII  (3/2)  = 1, 
GII  (-3/2)  = -2 

NEWARG  = OLDARG/2** (REDUCT* 2) 

FACTOR  = 2**REDUCT 

SORT  (OLDARG)  = SORT  (NEWARG)  * FACTOR 


Eight  initial  approximations  (over  the  .25  - — > 1 range 
are  given  in  the  reference  ("Computer  Approximations",  John  F. 
Hart  et  al  QA297,  C738) . 


An  iteration  of  Newtons  method  consists  of: 


NEWEST  = .5  * (OLDEST  + NEWARG/OLDEST) 
where  OLDEST  and  NEWEST  are  respectively)  the  old  and  new  estimates 
of  the  square  root  of  NEWARG.  Assuming  that  the  multiplicatoion 
by  .5  is  done  with  a multiply,  execution  time  is  therefore  assumed 
to  be  14  (see  below) . From  theory,  and  from  results  tabulated 
in  the  reference,  one  iteration  tends  to  double  the  precision 
of  the  estimate. 


Accordingly  certain  approximations  can  be  eliminated  from 
consideration 


i Eliminate 

(Hart's  Index) 

Since 

Accuracy 

Speed 

Storage 

292 

293 

Worse 

Same 

Same 

290 

0010  + 1 
iteration 

Worse 

Same 

Worse 

291 

§ 

0011  + l 
iteration 

Worse 

Same 

Worse 

Selection  of  an  initial  approximation  (from  the  remaining 
five)  is  based  on  the  required  precision  of  the  routine  taken  as 
a whole.  Examine  the  following  "Chart  of  Accuracy  and  Timings". 

Assuming  the  following  relative  execution  times  for 
add,  multiply  and  divide  instructions. 
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INDEX 


NUMBER  OF  ITERATIONS 


ADD  1 
MUL  3 
DIV  10 

tho  approximations  take  the  following  tines  and  yield  the  following 
accuracies. 


Approximation 
(Harts  Index) 

4 

Execution 

Time 

Precision  in 
Decimal  Digits 
(relative  error) 

0070 

4 

1.53 

0071 

8 

2.30 

0072 

12 

2.98 

0073 

16 

3.60 

0290 

18 

2.60 

0291 

22 

3.66 

0292 

24 

4.55 

0293 

24 

4.73 

We  are  targetting  48,  40,  32  and  24  bits  of  precision 
which  correspond,  respectively,  to  14.4,  12.0,  9.6  and  7.2  decimal  digit 

We  will  therefore  define  the  following  paths 


Path 

Index 

Number  of 

Iterations 

Approximate  Precision 
(Decimal/Binary) 


1 0073  1 

2 0071  2 

3 0072  2 

4 0073  2 


7.5  24.7 
10.1  33.3 
12.9  42.6 
15.3  50.5 


Now  consider  the  effect  of  order  of  computation  on  results 
of  Newton's  iteration  in  square  root  module. 


Newton's  iteration  can  oe  performed  in  more  than  one  manner. 


Consider 

A) 

Yn+1 

- 1/2  <Yn  + $-) 

n 

B) 

Vl 

- 1/2  (Y  - $-) 
n 

4.  X 

C) 

Yn+1 

- Yn  ♦ 1/2  ($-  - 
n 

V 
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Examine  the  following  two  cases:  (note  that  B'  xxxx  xxxx' 
means  the  binary  fraction  .xxxx  xxxx) . 

Case  I 

X ~ B ' 1000  0000  '*  21  (that  is  *1*} 


Y = B ' 1000  0001  ' * 21  (that  is  >*1‘) 
n 

X/Yn  = B'  1000  0000  ,*  2 1 (with  rounding) 


A)  - B«  1000-0000  « * 2a 

B)  = B*  1000  0000  ’ * 21 

C)  = B’  1000  0001  * * 21 


Case  II 


yn  = 


B'  1000  0000  < *2  1 (that  is  *1*) 
B'  1111  1111’  *2°  (that  is  •<'!’) 


= '1000  0001’  * 2J 


A)  = B»  1000  0001* 

B)  = B*  1000  0000’ 


C)  = B'  1000  0000’ 

Therefore,  the  last  iteration  will  be  done  using  "order"  B. 


Inputs,  Outputs  & Internal  Variables 


ARG  - input  argument 
VSQRT  - result  value 

SQRPTH-  selects  "algorithm*  to  be  used 
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VSQRT 
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VSQSUP 


2.3.47  Function  & Equations 

The  purpose  of  this  routine  is  to  perform  the  'range 
reduction'  required  hy  the  square  root  routine.  This  range 
reduction  consists  of  expressing  an  input  argument,  X,  as 

X = Y * 22n  where  Y is  between  .25  and  1 and 

n is  a positive  or  negative  integer  or  zero. 

For  a more  complete  description,  refer  to  both  the  VSQRT  docu- 
mentation and  the  comments  in  the  listing  of  the  routine. 

Inputs,  Outputs  & Internal  Variables 

First  argument  - 'X'  above  (input) 

Second  argument  - 'Y*  above  (output) 

Third  argument  - 2n  above  (output) 


! 


j VRTZYX 

I 

1 2.3.48  Function  & Equations 

S 

This  routine  implements  the  ROTZYX  routire  that  would  be 
implemented  in  an  airborne  computer.  It  differs  from  the 
5 ROTZYX  routine  only  in  that  it  contains  calls  to  the  VP  subroutine. 


3 

- si 

If. 

4 


! j 


t 


i 
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VRTXYZ 

2.3.49  Function  & Equations 

This  routine  implements  the  ROTXYZ  routine  that  would  be 
implemented  in  an  airborne  computer.  It  differs  from  the 
ROTXYZ  routine  only  in  that  it  contains  calls  to  the  VP  sub- 
routine . 


s 


I 

f 


I 

1 


t 


|i 

r 
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VMTM 


2.3.50  Function  & Equations 

This  routine  implements  the  "matrix  transpose  by  matrix" 
multiply  routine  that  would  be  implemented  in  an  airborne  com- 
puter. It  differs  from  the  MTM  routine  only  in  that  it  contains 
calls  to  the  VP  subroutine. 


-VM&TVC 


2.3.51  Function  & Equations 

This  routine  implements  the  MATVEC  routine  that  would  be 
implemented  in  an  airborne  computer.  It  differs  from  the  MATVEC 
routine  only  in  that  it  contains  calls  to  the  VP  subroutine. 


i 


\ 
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2.3.52  Function  & Equations 


This  routine  implements  the  matrix  multiply  routine  that 
would  be  implemented  in  an  airborne  computer.  It  differs  from 
the  MM  routine  only  in  that  it  contains  calls  to  the  VP  sub- 
routine . 
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3 • 0 PROGRAM  OPERATION 


3.1  Introduction 

The  simulator  is  operated  by  providing  it  with  PROFGEN's 
output  tape,  and  by  specifying  certain  inputs.  The  fil«s  required 
are  described  in  an  appendix  titled  "Local  Files",  the  inputs 
are  described  in  the  next  section,  and  sample  SCOPE  control  cards 
are  given  in  Appendix  G. 


3.2  Operator  Inputs 
a)  Inputs 

The  operator  input  required  consists  of 

i)  First  a title  card,  then 
ii)  One  or  more  cards  comprising  a FORTRAN  NAMELIST 

input  for  SIMPAR.  This  controls  all  the  parameters 
of  the  simulation.  The  SIMPAR  data  input  is 
described  in  the  following  section,  and  further 
explained  in  Appendix  C. 


The  operator  input  might  therefore  look  something  like  the 
following  sample: 


ITRNAV  = 4 $ 


$ SIMPAR 


PRNT  = .5,  IPC  = 27  * 0,  3 * 1, 


THIS  IS  A TITLE 


b)  NAMELIST  items  & Defaults 


The  following  variables  are  on  the  SIMPAR  Namelist  and 
can  be  input,  at  initialization,  into  the  system. 


Variable 

Units 

Default 

Description 

AQUANT 

radians 

0.0 

Quantization  units  for  attitude 

TQUANT 

radians 

0.0 

Quantization  units  for  torquing 
angle  for  a nav  cycle 

VQUANT 

ft/sec 

0.0 

Quantization  units  for  delta 
velocity 

START 

sec 

-1.E6 

Time  (on  input  tape)  at  which 

to  start  simulation 

STOP 

sec 

1.E6 

Time  (on  input  tape)  at  which 
to  stop  simulation 

PLOTIM 

sec 

1.E6 

Simulated  time  between  binary 
outputs 

PRNT 

sec 

1.E6 

Simulated  time  between  printouts 

RSTRT 

sec 

1.E6 

See  "Restart  Hook"  section 

TOLJRK 

ft/sec3 

1.E6 

CMINTG’s  tolerance  for  'jerk' 

see  CMINTG  documentation 

HSOTIM 

sec 

1.E6 

Time  (on-  input  tape)  at  which 
one  should  start  (ortho) 
normalization  of  bcdy-to-computa' 

tional  transformation . 

HSOINC 

sec 

1.E6 

Simulated  time  between  (ortho) 

normalizations  described  under 

HSOTIM 

SSXO 

radians 

0.0 

EUler  rotation  about  X of  trans- 
formation from  space  stable  to 

initial  earth  fixed 

SSYO 

radians 

0.0 

E.uler  rotation  about  Y of  trans- 
formation from  space  stable  to 

initial  earth  fixed 

ssz-o 

radians 

0.0 

Euler  rotation  about  Z of  trans- 
formation from  space  stable  to 

initial  earth  fixed 
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Variables  Units 


Default 


Description 


ITRNAV 

CPS 

0 

ITRATT 

CPS 

0 

INCOR 

none 

100 

IMUTYP 

none 

1 

IERLIM 

none 

10 

IPC 

none 

0 

IOPNLP 

none 

0 

CVD1 

sec"1 

6.E-2 

CVD  2 

-2 

sec 

1.62E-3 

CVD3 

-3 

sec  J 

1. 62Er*5 

ODX 

radians 

0,0 

ODY 

radians 

0.0 

ODZ 

radians 

0,0 

GAINS 

none 

see 

listing 

Rate  associated  with  navigation 
cycle?  default  is  ITRATT 
Rate  associated  with  attitude 
cycle;  default  is  l./DELT 
Number  of  navigation  cycles 
between  orthonormalization  of 
transformation  from  computational 
to  earth  fixed 

IMU  type  1 is  local  level,  2 is 
space  stable,  3 is  strapdown 
Error  limit 

See  IPC  list,  variables  descrip- 
tion, for  explanation 
Used  by  Local  Level  only.  0 
implies  closed  loop,  that  is, 

IMU  data  is  affected  by  misalign- 
ments caused  by  torquing  errors. 

1 implies  IMU  data  remains  unaf- 
fected . 

Vertical  damping  constant,  see 
INRNAV  description 
Vertical  damping  constant,  see 
INRNAV  description 

Vertical  damping  constant,  see 
INRNAV  description 
See  description  of  ODX 
See  description  of  ODY 
See  description  of  ODZ 
Gains  for  attitude  filter; 
default  based  on  values  for 
32  Hz  rate 


Variable  Units 


Default 


Description 


RSMAX 

radians 

see 

listing 

Residuals  tolerance  - see  ATTFIL 

documentation 

DRSMAX 

radians 

see 

listing 

Delta-Residuals  tolerance  - 

see  ATTFIL  documentation 

ITRFIL 

CPS 

32 

Rate  associated  with  attitude 
filter  cycle 

SQRPTH 

none 

4 

Square  root  algorithm  "path" 

SINPTH 

none 

4 

Sine  algorithm  "path" 

COSPTH 

none 

4 

Cosine  algorithm  "path" 

ATNPTH 

none 

4 

Arc  tangent  algorithm  "path" 

ROUND 

none 

1 

1 Implies  round,  0 implies 
truncate  only 

FRACTL 

bits 

48 

Length  of  simulated  fraction 

EXPOL 

bits 

8 

Length  of  simulated  exponent 

c)  Using  PROFGEN  in  Conjunction  With  Simulator 

The  reader  is  assumed  to  be  familiar  with  PROFGEN  and 
its  usage.  PROFGEN  produces  a.  tape  which  basically  has 
the  specific  forces  and  attitude  angles  that  would  be  sensed  by 
an  IMU.  Several  requirements  should  be  kept  in  mind  when  run- 
ning PROFGEN  for  use  in  conjunction  with  the  simulator. 

First,  the  output  rate  (DTO  of  the  PROFGEN  input)  must  be 
the  same  for  all  trajectory  segments.  This  rate  must  be  an 
integral  multiple  of  the  attitude  rate  one  wishes  to  run  in  the 
simulator.  It  is  suggested  that  these  rates  be  limited  to 
binary  multiples  of  1 CPS  (cycle  per  second),  that  is  2,  4,  8, 
16,  32,  64,  128  etc. 

Second,  local  level  wander  azimuth  should  be  specified 
for  the  PROFGEN  run  (LLMECH  ■ 1) . 

Third,  PROFGEN' s RESTART  should  not  be  used;  that  is, 
RESTART  should  equal  zero  for  all’.trajectory  segments. 


Fourth,  PROFGEN ' s ERROR  parameter  must  be  small  enough 
to  keep  PROFGEN* s ’integration  error*  below  the  level  one  is 
trying  to  measure  in  the  simulator.  A value  of  l.E-9  has  proved 
satisfactory  during  testing. 

3.3  Simulator  Outputs 

a)  Binary  Output  File 

The  'binary'  output  of  the  simulator  (suitable  for  post 
processing)  is  written  into  "local  file"  TAPE30  (Fortran  logical 
unit  30) . The  format  of  this  output  is  described  in  an  appendix. 
In  general,  it  contains  the  differences  between 

i)  the  outputs  of  the  simulator 

ii)  the  same  physical  quantities  as  i)  except  that 
ii)  was  computed  by  PROFGEN. 

b)  Printed  Output 

The  printed  output  of  the  simulator,,  shown  in  Figure  7, 
consists  of  three  parts: 

i)  the  parameters  describing  the  trajectory  segments 
by  the  simulator 

ii)  The  parameters  controlling  the  simulation 

iii)  Periodic  printouts. 

The  next  three  exhibits  are  sample  outputs,  as  follows: 

i)  a complete  8 sec  local  level  run,  with  attitude 
filter,  with  printouts  every  half  second.  The 
units  of  the  periodic  printouts  are  as  follows: 

Degrees  - Latitude,  longitude,  alpha,  roll,  pitch, 
yaw  (yaw  is  actually  heading) , all  nine 
filtered  attitude  parameters  (yaw  here 
includes  alpha) . 

None  - Gains  pointers  for  each  of  filtered  roll, 
filtered  pitch  and  filtered  yaw 

Feet  - Altitude 

Feet/sec  - VX  (up) , VY  (east) , VZ  (north) 
ii)  the  second  page  of  a space  stable  run. 
iii)  the  second  page  of  a strapdown  run. 
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Figure  7.  NUMSIM  Output  Exhibits  (Continued) 
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None  of  the  printouts  that  can  be  generated  by  DOUT  are 
in  these  samples.  The  units  of  the  DOUT  printout  can  be  found 
in  the  variable  listings, 

'3.4  Operational  Aids 

a)  Cautions  and  Other  Miscellaneous  Advice 

1)  Read  the  section  on  using  PROFGEN;  the  simulator 
assumes  (and  does  not  check)  all  of  the  mentioned 
conditions . 

2)  PROFGEN  has  shown  a tendency  to  do  strange  things 
with  the  low  order  bits  of  the  TIME  on  the  first 
few  seconds,  specifically  a DTO  of  .0078125  may  come 
out  .007812499999  or  .00781250001.  This  condition 
can  be  circumvented  by  specifying  a slightly 
smaller  number  (,0078124  in  our  example)  for 
quantities  such  as  PRNT,  PLOTIM,  HSOINC. 

3)  The  GAINS  in  the.  Block  Data  subprogram  are  designed 
for  32  CPS,  and  will  work  miserably  at  any  other 
rate. 

4)  One  cannot  get  printout  or  binary  output  at  a rate 
higher  than  that  the  navigation  rate. 

5)  No  "range  checking"  is  performed  on  the  SIMPAR 
inputs;  that  is,  setting  IMUTYP  to  4 or  IPC  (26) 
to  10  might  produce  unpredictable  results. 

b)  Error  Messages 

The  error  messages  that  can  be  produced  by  the  simula- 
tor is  as  follows; 

1)  Timing  error  message  - see  attached; generates 
error  level  5 

2)  Tape  read  error  message  - (not  illustrated)  generates 
error  level  5 

3)  End  of  input  file  - see  attached  generates  error 
level  10 
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SIMULATION  TERMINATED:  ERROR  SEVERITY  Of-  S.  ERROR  LIMIT  IS 


^PENDICES 


A.  SUBROUTINE  CALL  STRUCTURE* 


DOUT 

PRINTR 

PLTAPE 

CMINTG 

OUTUNI 


TORCOR 

GRAV 

ANGVL2 

POSVEL 


•BLOCK  DATA  SUBPROGRAM  IS  NOT  EXPLICITLY  CALLED,  AND  IS  NOT 
SHOWN.  LIBRARY  SUBROUTINES  (e.g.,  TRIGONOMETRIC  FUNCTIONS, 
MATRIX  MULTIPLY,  ETC.)  ARE,  LIKEWISE,  NOT  SHOWN. 


B.  Delivery  and,  installation  ■ 

Two  versions  of  the  simulator  are  being  delivered.  One 
has  the  "variable  precision"  feature  implemented?  the  other  does 
not.  The  former  requires  substantially  (order  of  magnitude 
estimate:  10  times}  more  execution  time  than  the  latter. 

Listing  of  both  programs  are  presented  in  Volume  IV. 


The 

software  is  being  delivered 

in  sections  as  follows: 

1) 

NUMEXC 

11) 

SDFLT 

2) 

NUMIMU 

12) 

VUMEXC 

3) 

NUMFLT 

13) 

VUMFLT 

4) 

SUBS1 

14) 

VUBS1 

5) 

SUBS2 

15} 

VUBS3 

6) 

SSIMU 

16) 

VPREC 

7) 

SSFLT 

17) 

VSFLT 

8) 

LLIMU 

18) 

VLFLT 

9) 

LLFLT 

19) 

VDFLT 

10) 

SDIMU 

The 

makeup  of  each  version 

can  ' 

be  seen  in  the  attached 

table. 

Note  that 

i)  "Identical"  implies  the  code  for  some  section  of  the 
two  simulators  is  identical 

ii)  "Same  Names"  implies  the  code  differs  only  by  the 
inclusion  of  the  precision  varying  subroutines 


iii)  All  sections  whose  names  and  with  ’PLT*  are  flight 
code,  as  are  SUBS1,  SUBS2,  VUBS1  and  VUBS3. 


lllUiIl0lllUli8llUUUUllll!IUUIttllllUtlSllUlllll!lllllUU!l-U!U!iU(l!llllQllli!lllllU!U,l!lll,!l!JII!!,P!!l!IIIU,i!ll! 


< 

Without-  Precision 

With  Precision 

NUMEX 

Main  program 

VUMEXC 

Main  program 

f 

NUMIMU 

BLOCK  DATA  PLTAPE 
INREC  DOUT 

OUTUNI  CMINTG 

PRINTR 

NUMIMU 

Identical 

NUMFLT 

TORCOR  INXNIT 
INRNAV  ATTFIL 
POSVEL 

GRAV 

ANGVL2 

ATUDE 

VUMFLT 

Same  names 

SUBSl 

ORTHO 

DCMUPD 

VUBS1 

Same  names 

» 

♦ 

SUBS  2 

matvec  qntiz 

MTM 

MM 

ROTXYZ 

MATRAN 

ROTZYX 

SUBS2 

Identical 

VUBS3 

VRTXYZ 

VRTZYX 

VMTM 

VMATVC 

VMM 

VPREC 

VP 

VPINIT 

VSIN 

VCOS 

VSINCO 

VAT AN 2 

VSQRT 

VSQSUP 

SSXMU 

SSATUD 

SSINTG 

SSIMU 

Identical 

SSFLT 

SSINIT 

SSTFRM 

SSATT 

VSFLT 

Same  names 

1 p 
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Without  Precision 


With  Precision 


LLIMU 

LLA.TUD 

LLINTG 

LLIMU 

Identical 

LLFLT 

LLINIT 

LLATT 

VLFLT 

Same  names 

SDIMU 

sdatud 

SDINTG 

SDIMU 

Identical 

SDFLT 

SDINIT 

SDATT 

VDFLT 

Same  names 

HSINTG 

HSINT2 

i 


! 1 


I 

J 

I 


; 


I 

! 

i 

| 

I 


C.  Sample  Set  of  Simulator  inputs 

Desired;  strapdown  system 

attitude  at  128  CPS 
navigation  at  4 CPS 
attitude  filter  at  32  CPS 
3rd  order  quaternion  update 
default  rates,  tolerances 

Navigation  DCM  orthonomalize  every  nav  cycle 
No  quantization 

printout  every  5 simulated  seconds 
binary  outputs  every  .5  seconds 
quaternion  normalize  every  attitude  cycle 
second  order  navigation  DCM  update 
higher  ’order'  algorithms,  exact  angular  velocity 
Assumes  PROFGEN  starting  time  was  0. 

Card  column  2 

$SIMPAR  IMUTYP=3 , ITRATT=128 , ITRNAV=4 , 

ITRFIL=32 , IPC=24*0 ,1,2, 1,1, 1,1, INCOR=l , 

PRNT-5 . , PLTIME=0 . , PL0TIM= . 5 , HSOTIM=0 , 

HSOINC=. 007812$ 
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D.  Assumed  Format  of  Binary  Input 


Record 

1 

2 

3 

4 thru  n 


Contents 


11  TIME 
2}  LAT$T 

3)  LONG$T 

4)  ALF$T 

5)  HB 

6)  ETA$T  (1) 

7)  ETA$T  (2) 

8)  ETA$T  (3) 

9)  V$T  (1) 

10)  V$T  (2) 

11)  V$T  (3) 

12)  SF$T  (1) 

13)  SF$T  (2) 

14)  S'F$T  (3) 


Date  and  Time  from  PROFGEN 

PROFGEN*s  $PRDATA 

PROFGEN »s  $PASDATA 

14  word  records  tsee  variable 
listing) 
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■ 


■1 


m 

,3 

i 
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"Local  Files”  Required  By  Simulator  Under  CPC  INTERCOM/SCOPE 


TAPES  - operator  input  to  program 
TAPE6  - printed  output 

TAPE20  - binary  input  file  (input  from  PRQFGEN) 
TAPE 30  - binary  output  file  from  simulator 
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Format  of  Binary  Output 
Record 


1 

2 


Contents 

80character  title 
11  words  as  follows 


1) 

TIME 

time 

2) 

DLAT 

latitude  difference 

3) 

DLONG  - 

longitude  difference 

4) 

DALF 

alpha  difference 

5) 

DH 

altitude  difference 

6) 

DV  (1)  - 

vertical  velocity  difference 

7) 

DV  (2)  - 

east  velocity  difference 

8) 

DV  (3)  - 

north  velocity  difference 

9) 

DETA(l) - 

roll  difference 

10) 

DETA (2 ) — 

pitch  difference 

-U) 

DETA ( 3 ) - 

heading  difference 

G.  Sample  SCOPE  Control  Cards  (assuming  TAPE  5 has  been  equated 


ATTACH,  PROG , NUMSIM, CY=8 
VSN , TAPE2  0=L05  3 7 0 . 

LABEL , TAPE 20 , R,L=TENMINJOB , D=HD, NORING 


PROG. 


DISPOSE, TAPE6 , PR=IBD 


*EOR 


simulator  input 


H.  RESTART  HOOK 

The  Simulator  has  been  designed  with  the  potential  for  the 
easy  inclusion  of  a checkpoint/restart  facility.  The  key  to  this 
facility  is  the  fact  that  all  variables  that  are  carried  "from 
navigation  cycle  to  navigation  cycle"  are  stored  in  common  blocks. 

The  checkpointing  can  therefore  be  accomplished  by  adding  a 
subroutine.  The  subroutine  could  be  called  periodically  from  the 
exec  (one  could  control  the  timing  with  RSTRT,  which,  currently, 
is  unused1*  and  would  write  out  (on  a tape  presumably)  the  contents 
of  the  COMMON  blocks. 

The  restart  procedure  would  therefore  consist  of 

a)  Read  in  an  operator  input  describing  the  simulated  time 
of  which  restart  is  to  occur 

b)  Find,  in  the  checkpoint  file,  the  data  corresponding  to 
the  time  from  step  a) . Read  this  data  in  to  the  COMMON 
blocks 

c)  Find,  in  the  binary  input  tape,  the  time  corresponding 

to  the  time  from  step  a) . Leave  the  input  tape  positioned 
after  this  record 

d)  Resume  computations  at  the  beginning  of  the  next  naviga- 
tion cycle. 
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